Robofun 機器人論壇

 找回密碼
 申請會員
搜索
熱搜: 活動 交友 discuz
樓主: tommylin

arduino 的學習之路..( Part. 1~11 )

[複製鏈接]
發表於 2011-2-25 23:52:36 | 顯示全部樓層
回復 40# vegewell

您要不要另外開個主題。這樣比較好看到您老做的進度。
nichal 該用戶已被刪除
發表於 2011-2-26 17:12:53 | 顯示全部樓層
回復 37# vegewell


    就像G大說的
我是建議用MOSFET+H_Bridge控制
因為繼電器控制除了反應較慢(機械結構延遲)
還有火花的問題(接點容易因火花損耗造成接觸不良)
除非考慮隔離控制的需求
不然是不建議這樣用
 樓主| 發表於 2012-9-7 17:22:51 | 顯示全部樓層
由於Arduino 整個系列全部買齊了,
接下來會包含 Arduino UNO-R3, Mini Pro, MEGA ..
所以改標題了~

Part 9: 有刷馬達的控制.
準備元件..
1. 有刷馬達.
2. 馬達控制器( pololu-Qik2s12v10 )

Qik2s12v10

Qik2s12v10


原始程式(來自 Pololu 官網)
  1. /*
  2. Required connections between Arduino and qik 2s12v10:

  3.       Arduino   qik 2s12v10
  4. ---------------------------
  5.           GND - GND
  6. Digital Pin 2 - TX
  7. Digital Pin 3 - RX
  8. Digital Pin 4 - RESET

  9. DO NOT connect the 5V output on the Arduino to the 5V output on the qik 2s12v10!
  10. */

  11. #include <SoftwareSerial.h>
  12. #include <PololuQik.h>

  13. PololuQik2s12v10 qik(2, 3, 4);

  14. void setup()
  15. {
  16.   Serial.begin(115200);
  17.   Serial.println("qik 2s12v10 dual serial motor controller");
  18.   
  19.   qik.init();
  20.   
  21.   Serial.print("Firmware version: ");
  22.   Serial.write(qik.getFirmwareVersion());
  23.   Serial.println();
  24. }

  25. void loop()
  26. {
  27.   for (int i = 0; i <= 127; i++)
  28.   {
  29.     qik.setM0Speed(i);
  30.     if (abs(i) % 64 == 32)
  31.     {
  32.       Serial.print("M0 current: ");
  33.       Serial.println(qik.getM0CurrentMilliamps());
  34.     }
  35.     delay(5);
  36.   }

  37.   for (int i = 127; i >= -127; i--)
  38.   {
  39.     qik.setM0Speed(i);
  40.     if (abs(i) % 64 == 32)
  41.     {
  42.       Serial.print("M0 current: ");
  43.       Serial.println(qik.getM0CurrentMilliamps());
  44.     }
  45.     delay(5);
  46.   }

  47.   for (int i = -127; i <= 0; i++)
  48.   {
  49.     qik.setM0Speed(i);
  50.     if (abs(i) % 64 == 32)
  51.     {
  52.       Serial.print("M0 current: ");
  53.       Serial.println(qik.getM0CurrentMilliamps());
  54.     }
  55.     delay(5);
  56.   }

  57.   for (int i = 0; i <= 127; i++)
  58.   {
  59.     qik.setM1Speed(i);
  60.     if (abs(i) % 64 == 32)
  61.     {
  62.       Serial.print("M1 current: ");
  63.       Serial.println(qik.getM1CurrentMilliamps());
  64.     }
  65.     delay(5);
  66.   }

  67.   for (int i = 127; i >= -127; i--)
  68.   {
  69.     qik.setM1Speed(i);
  70.     if (abs(i) % 64 == 32)
  71.     {
  72.       Serial.print("M1 current: ");
  73.       Serial.println(qik.getM1CurrentMilliamps());
  74.     }
  75.     delay(5);
  76.   }

  77.   for (int i = -127; i <= 0; i++)
  78.   {
  79.     qik.setM1Speed(i);
  80.     if (abs(i) % 64 == 32)
  81.     {
  82.       Serial.print("M1 current: ");
  83.       Serial.println(qik.getM1CurrentMilliamps());
  84.     }
  85.     delay(5);
  86.   }
  87. }
複製代碼
 樓主| 發表於 2012-9-7 17:39:39 | 顯示全部樓層
一個小插曲.. 本來要先做 IMU 的學習..
但是... 買了一個超級爆笑的 IMU ... 來自對岸 = =
拍了照片..給大家笑一笑, 花了我不少錢學到教訓了@@

太誇張的QC

太誇張的QC


您看出哪裡有問題了嗎^^
 樓主| 發表於 2012-9-7 17:42:48 | 顯示全部樓層
這張應該就看出來了吧 = =

蝦瞇碗糕

蝦瞇碗糕
 樓主| 發表於 2012-9-7 21:47:41 | 顯示全部樓層
焊好接腳, 主角( qik 2s12v10 )登場..

qik2s12v10

qik2s12v10
發表於 2012-9-7 22:05:44 | 顯示全部樓層
一個小插曲.. 本來要先做 IMU 的學習..
但是... 買了一個超級爆笑的 IMU ... 來自對岸 = =
拍了照片..給大 ...
tommylin 發表於 2012-9-7 17:39


咦?
字顛倒
又沒焊接?
 樓主| 發表於 2012-9-7 23:13:26 | 顯示全部樓層
回復 47# mzw2008

您真內行..字顛倒了, 呵呵~
 樓主| 發表於 2012-9-7 23:17:35 | 顯示全部樓層
本帖最後由 tommylin 於 2012-9-10 17:11 編輯

先用 Arduino UNO - R3 頂著用..
義大利製造, 品質不是對岸能比的@@
Arduino UNO-R3.JPG

要改裝的戰車...
Tank-1.JPG

分屍後~
Tank-2.JPG

這台是會打出bb彈的喔~

影片:


接線圖:
Wii wire with UNO.JPG
 樓主| 發表於 2012-9-8 01:23:56 | 顯示全部樓層
本帖最後由 tommylin 於 2012-9-9 12:51 編輯

PART 10: Wii motion plus + Nunchuck
我知道這已經過時很久了,
但是我蹺課太久了,現在捕回來@@
Wii.jpg

找到下列程式有一個重點,就是經過了 "Kalman Filter"的計算,
這樣才能夠實際應用, 不然一堆雜訊是沒辦法用的.
但是如果要更好的效果, 就要選用 9DOF IMU,
搭配Compass一起計算...這是後面的課題了..
  1. // Modified Kalman code using the Y-Axis from a Wii MotionPlus and the X and Z from a Nunchuck.
  2. // Nunchuck joystick provides corrections - X for Gyro, Y for Accelerometer
  3. // Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.
  4. // Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904
  5. // See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html
  6. // Kalman Code Originally By Jordi Munoz... //Si deseas la version en Espanol, no dudes en escribirme...
  7. //
  8. // Altered by Adrian Carter <adrian@canberrariders.org.au> to work with WiiM+ and Nunchuck

  9. #include <math.h>
  10. #include <Wire.h>
  11. /////////////////////////////////////////
  12. #define NUMREADINGS 5 //Gyro noise filter
  13. // test for 3 axis
  14. int readingsX[NUMREADINGS];
  15. int readingsY[NUMREADINGS];
  16. int readingsZ[NUMREADINGS];                    // the readings
  17. int totalX = 0;
  18. int totalY = 0;
  19. int totalZ = 0;                                   // the running total
  20. int averageX = 0;                                  // the average
  21. int averageY = 0;
  22. int averageZ = 0;
  23. int lastaverageZ =0;
  24. // End of 3 axis stuff
  25. int readings[NUMREADINGS];                    // the readings from the analog input (gyro)
  26. int index = 0;                                    // the index of the current reading
  27. int total = 0;                                    // the running total
  28. int average = 0;                                  // the average
  29. int inputPin =0;                                  // Gyro Analog input
  30. //WM+ stuff
  31. byte data[6]; //six data bytes
  32. int yaw, pitch, roll; //three axes
  33. int yaw0, pitch0, roll0; //calibration zeroes
  34. ///////////////////////////////////////

  35. float        dt        = .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)
  36. int mydt = 20; //in ms.
  37. static float                P[2][2] = { //(Kalman)

  38.   {
  39.     1, 0   }
  40.   , //(Kalman)
  41.   {
  42.     0, 1   }
  43.   ,//(Kalman)
  44. }; //(Kalman)
  45. // extra 2 axis..
  46. static float                P1[2][2] = {  { 1, 0 } ,  { 0, 1 } ,};
  47. static float                P2[2][2] = {  { 1, 0 } ,  { 0, 1 } ,};
  48. static float                P3[2][2] = {  { 1, 0 } ,  { 0, 1 } ,}; //(Kalman)


  49. /*
  50. * Our two states, the angle and the gyro bias.  As a byproduct of computing
  51. * the angle, we also have an unbiased angular rate available.   These are
  52. * read-only to the user of the module.
  53. */
  54. float                        angle; //(Kalman)
  55. float                         angleX;
  56. float                         angleY;
  57. float                         angleZ;
  58. float                        q_bias; //(Kalman)
  59. float                         q_bias_X;
  60. float                         q_bias_Y;
  61. float                         q_bias_Z;
  62. float                         rate_X;
  63. float                         rate_Y;
  64. float                         rate_Z;
  65. float                         q_m_X; //(Kalman)
  66. float                         q_m_Y;
  67. float                         q_m_Z;
  68. float                         rotationZ;
  69. float                        rate; //(Kalman)
  70. float                         q_m; //(Kalman)

  71. // adjusts
  72. int gyro_adjust = 1;
  73. int accel_adjust = 128;

  74. int joy_x_axis = 0; //NunChuck Joysticks potentiometers
  75. int joy_y_axis = 0;
  76. int ax_m=0;        //NunChuck X acceleration
  77. // Re-added Y Axis
  78. int ay_m=0;        //NunChuck Y acceleration
  79. int az_m=0;        //NunChuck Z acceleration
  80. byte outbuf[6];                // array to store arduino output
  81. int cnt = 0;                //Counter
  82. unsigned long lastread=0;

  83. /*
  84. * R represents the measurement covariance noise.  In this case,
  85. * it is a 1x1 matrix that says that we expect 0.3 rad jitter
  86. * from the accelerometer.
  87. */
  88. float        R_angle        = .3; //.3 deafault, but is adjusted externally (Kalman)


  89. /*
  90. * Q is a 2x2 matrix that represents the process covariance noise.
  91. * In this case, it indicates how much we trust the acceleromter
  92. * relative to the gyros.
  93. */
  94. static const float        Q_angle        = 0.001; //(Kalman)
  95. static const float        Q_gyro        = 0.003; //(Kalman)

  96. void switchtonunchuck(){
  97. /*  PORTE ^= 32; // Toggle D3 LOW
  98.   PORTG |= 32; // Force D4 HIGH */ // Fast switching of pins... Arduino MEGA specific...
  99. digitalWrite(3, LOW);
  100. digitalWrite(4, LOW);
  101. digitalWrite(4, HIGH);

  102. }

  103. void switchtowmp(){
  104. /*  PORTG ^= 32; // Toggle D4 LOW
  105.   PORTE |= 32; // Force D3 HIGH */ // Fast switching of pins... Arduino MEGA Specific pin maps, so switched back to 'normal' code for example
  106. digitalWrite(3, LOW);
  107. digitalWrite(4, LOW);
  108. digitalWrite(3, HIGH);
  109. }

  110. void nunchuck_init () //(nunchuck)
  111. {
  112. // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles
  113. Wire.beginTransmission (0x52);        // transmit to device 0x52
  114.   Wire.write (0xF0);                // sends memory address
  115.   Wire.write (0x55);                // sends sent a zero.
  116.   Wire.endTransmission ();        // stop transmitting
  117.   delay(100);
  118.   Wire.beginTransmission (0x52);        // transmit to device 0x52
  119.   Wire.write (0xFB);                // sends memory address
  120.   Wire.write (0x00);                // sends sent a zero.
  121.   Wire.endTransmission ();        // stop transmitting
  122. }
複製代碼
//接後面
 樓主| 發表於 2012-9-9 12:53:10 | 顯示全部樓層
  1. void setup()
  2. {
  3.   Serial.begin(115200);
  4. /*  DDRG |= 32; // Force D3 to Output // Arduino MEGA "fast" pin config. Reverted to 'normal' for example post.
  5.   DDRE |= 32; // Force D4 to Output
  6.   PORTG ^= 32; // Toggle D3 HIGH
  7.   PORTE ^= 32; // Toggle D4 HIGH */

  8.   pinMode(3, OUTPUT);
  9.   pinMode(4, OUTPUT);
  10.   digitalWrite(3, HIGH);
  11.   digitalWrite(4, HIGH);

  12.   Wire.begin ();                // join i2c bus with address 0x52 (nunchuck)
  13.   switchtowmp();
  14.   wmpOn();
  15.   calibrateZeroes();
  16.   switchtonunchuck();
  17.   nunchuck_init (); // send the initilization handshake

  18.   for (int i = 0; i < NUMREADINGS; i++)
  19.     readings[i] = 0;                            // initialize all the readings to 0 (gyro average filter)
  20. }

  21. void send_zero () //(nunchuck)
  22. {
  23.   Wire.beginTransmission (0x52);        // transmit to device 0x52 (nunchuck)
  24.   Wire.write (0x00);                // sends one byte (nunchuck)
  25.   Wire.endTransmission ();        // stop transmitting (nunchuck)
  26. }

  27. void wmpOn(){
  28. Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
  29. Wire.write(0xfe); //send 0x04 to address 0xFE to activate WM+
  30. Wire.write(0x04);
  31. Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
  32. }

  33. void wmpOff(){
  34. Wire.beginTransmission(82);
  35. Wire.write(0xf0);//address then
  36. Wire.write(0x55);//command
  37. Wire.endTransmission();
  38. }

  39. void wmpSendZero(){
  40. Wire.beginTransmission(0x52);//now at address 0x52
  41. Wire.write(0x00); //send zero to signal we want info
  42. Wire.endTransmission();
  43. }

  44. void calibrateZeroes(){
  45. for (int i=0;i<10;i++){
  46. wmpSendZero();
  47. Wire.requestFrom(0x52,6);
  48. for (int i=0;i<6;i++){
  49. data[i]=Wire.read();
  50. }
  51. yaw0+=(((data[3] >> 2) << 8)+data[0])/10;//average 10 readings
  52. pitch0+=(((data[4] >> 2) << 8)+data[1])/10;// for each zero
  53. roll0+=(((data[5] >> 2) << 8)+data[2])/10;
  54. }
  55. }

  56. void receiveData(){
  57. wmpSendZero(); //send zero before each request (same as nunchuck)
  58. Wire.requestFrom(0x52,6); //request the six bytes from the WM+
  59. for (int i=0;i<6;i++){
  60. data[i]=Wire.read();
  61. }
  62. //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  63. //for info on what each byte represents
  64. yaw=((data[3] >> 2) << 8)+data[0]-yaw0;
  65. pitch=((data[4] >> 2) << 8)+data[1]-pitch0;
  66. roll=((data[5] >> 2) << 8)+data[2]-roll0;
  67. }

  68. void receiveRaw(){
  69. wmpSendZero(); //send zero before each request (same as nunchuck)
  70. Wire.requestFrom(0x52,6);//request the six bytes from the WM+
  71. for (int i=0;i<6;i++){
  72. data[i]=Wire.read();
  73. }
  74. yaw=((data[3] >> 2) << 8)+data[0];
  75. pitch=((data[4] >> 2) << 8)+data[1];
  76. roll=((data[5] >> 2) << 8)+data[2];
  77. }
複製代碼
//接後面
 樓主| 發表於 2012-9-9 12:55:29 | 顯示全部樓層
  1. void loop()
  2. {
  3.   if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
  4.     lastread = millis();
  5.     total += readings[index];
  6.     totalX += readingsX[index];
  7.     totalY += readingsY[index];
  8.     totalZ += readingsZ[index];                   // add the reading to the total
  9.     switchtowmp();
  10.     receiveData();
  11.     switchtonunchuck();
  12.     readings[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
  13.     readingsX[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
  14. //Continued
  15.     readingsX[index] = int(roll/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
  16.     readingsY[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
  17.     readingsZ[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
  18. //    receiveRaw(); // Provides unbiased output from WM+ (i.e, without calibrationZeros)


  19.     index = (index + 1);                          // advance to the next index

  20.     if (index >= NUMREADINGS)                   // if we're at the end of the array...
  21.         index = 0;                                    // ...wrap around to the beginning

  22.     average = (total / NUMREADINGS) - 511;
  23.     averageX = (totalX / NUMREADINGS) - 511;
  24.     averageY = (totalY / NUMREADINGS) - 511;
  25.     averageZ = (totalZ / NUMREADINGS) - 511;    // calculate the average of gyro input

  26.     q_m=((average*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
  27.     q_m_X=((averageX*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
  28.     q_m_Y=((averageY*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
  29.     q_m_Z=((averageZ*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick

  30.     /* Unbias our gyro */
  31.     const float        q_X = q_m_X - q_bias_X; //(Kalman)
  32.     const float q_Y = q_m_Y - q_bias_Y;
  33.     const float q_Z = q_m_Z - q_bias_Z;
  34.     const float        q = q_m - q_bias; //(Kalman)

  35.     const float                Pdot[2 * 2] = {
  36.         Q_angle - P[0][1] - P[1][0],        /* 0,0 */ //(Kalman)
  37.         - P[1][1],                /* 0,1 */
  38.         - P[1][1],                /* 1,0 */
  39.         Q_gyro                                /* 1,1 */
  40.     };

  41.      const float        PdotX[2 * 2] = {
  42.         Q_angle - P1[0][1] - P1[1][0],        /* 0,0 */ //(Kalman)
  43.         - P1[1][1],                          /* 0,1 */
  44.         - P1[1][1],                          /* 1,0 */
  45.         Q_gyro                                /* 1,1 */
  46.     };

  47.     const float        PdotY[2 * 2] = {
  48.         Q_angle - P2[0][1] - P2[1][0],        /* 0,0 */ //(Kalman)
  49.         - P2[1][1],                          /* 0,1 */
  50.         - P2[1][1],                          /* 1,0 */
  51.         Q_gyro                                /* 1,1 */
  52.     };

  53.     const float        PdotZ[2 * 2] = {
  54.         Q_angle - P3[0][1] - P3[1][0],        /* 0,0 */ //(Kalman)
  55.         - P3[1][1],                          /* 0,1 */
  56.         - P3[1][1],                          /* 1,0 */
  57.         Q_gyro                                /* 1,1 */
  58.     };

  59.     /* Store our unbias gyro estimate */
  60.     rate = q; //(Kalman)
  61.     rate_X = q_X; //(Kalman)
  62.     rate_Y = q_Y;
  63.     rate_Z = q_Z;

  64.     /*
  65.          * Update our angle estimate
  66.               * angle += angle_dot * dt
  67.               *         += (gyro - gyro_bias) * dt
  68.               *         += q * dt
  69.               */
  70.     angle += q * dt; //(Kalman)
  71.     angleX += q_X * dt; //(Kalman)
  72.     angleY += q_Y * dt;
  73.     angleZ += q_Z * dt;

  74.     /* Update the covariance matrix */
  75.     P[0][0] += Pdot[0] * dt; //(Kalman)
  76.     P[0][1] += Pdot[1] * dt; //(Kalman)
  77.     P[1][0] += Pdot[2] * dt; //(Kalman)
  78.     P[1][1] += Pdot[3] * dt; //(Kalman)

  79.     P1[0][0] += PdotX[0] * dt; //(Kalman) X axis
  80.     P1[0][1] += PdotX[1] * dt; //(Kalman)
  81.     P1[1][0] += PdotX[2] * dt; //(Kalman)
  82.     P1[1][1] += PdotX[3] * dt; //(Kalman)

  83.     P2[0][0] += PdotY[0] * dt;//y axis
  84.     P2[0][1] += PdotY[1] * dt;
  85.     P2[1][0] += PdotY[2] * dt;
  86.     P2[1][1] += PdotY[3] * dt;

  87.     P3[0][0] += PdotZ[0] * dt;//y axis
  88.     P3[0][1] += PdotZ[1] * dt;
  89.     P3[1][0] += PdotZ[2] * dt;
  90.     P3[1][1] += PdotZ[3] * dt;

  91.     Wire.requestFrom (0x52, 6);        // request data from nunchuck

  92.     while (Wire.available ()) //(NunChuck)
  93.     {
  94.         outbuf[cnt] = Wire.read ();        // receive byte as an integer //(NunChuck)
  95.         cnt++;//(NunChuck)
  96.     }

  97.     send_zero (); // send the request for next bytes
  98.     // If we recieved the 6 bytes, then print them //(NunChuck)
  99.     if (cnt >= 5) //(NunChuck)
  100.     {
  101.         print (); //(NunChuck)
  102.     }
  103.     cnt = 0; //(NunChuck)

  104.     R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick


  105.     const float                angle_m = atan2( ax_m, az_m ); //(Kalman)
  106.     const float                angle_err = angle_m - angle; //(Kalman)
  107.     const float                C_0 = 1; //(Kalman)
  108.     const float                PCt_0 = C_0 * P[0][0];  //(Kalman)
  109.     const float                PCt_1 = C_0 * P[1][0]; //(Kalman)
  110.     const float                E =R_angle+ C_0 * PCt_0; //(Kalman)
  111.     const float                K_0 = PCt_0 / E; //(Kalman)
  112.     const float                K_1 = PCt_1 / E; //(Kalman)
  113.     const float                t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */

  114.     const float                t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


  115.     P[0][0] -= K_0 * t_0; //(Kalman)
  116.     P[0][1] -= K_0 * t_1; //(Kalman)
  117.     P[1][0] -= K_1 * t_0; //(Kalman)
  118.     P[1][1] -= K_1 * t_1; //(Kalman)
  119.     angle        += K_0 * angle_err; //(Kalman)
  120.     q_bias        += K_1 * angle_err; //(Kalman)

  121.           const float                angle_mX = atan2( ax_m, az_m ); //(Kalman) X axis
  122.     const float                angle_errX = angle_mX - angleX; //(Kalman) X axis
  123.     const float                C_0_1 = 1; //(Kalman) X axis
  124.     const float                PCt_0_1 = C_0_1 * P[0][0];  //(Kalman)
  125.     const float                PCt_1_1 = C_0_1 * P[1][0]; //(Kalman)
  126.     const float                E_1 =R_angle+ C_0_1 * PCt_0_1; //(Kalman)
  127.     const float                K_0_1 = PCt_0_1 / E_1; //(Kalman)
  128.     const float                K_1_1 = PCt_1_1 / E_1; //(Kalman)
  129.     const float                t_0_1 = PCt_0_1; /* C_0_1 * P[0][0] + C_1 * P[1][0] (Kalman) */
  130.     const float                t_1_1 = C_0_1 * P[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


  131.     P1[0][0] -= K_0_1 * t_0_1; //(Kalman)
  132.     P1[0][1] -= K_0_1 * t_1_1; //(Kalman)
  133.     P1[1][0] -= K_1_1 * t_0_1; //(Kalman)
  134.     P1[1][1] -= K_1_1 * t_1_1; //(Kalman)
  135.     angleX        += K_0_1 * angle_errX; //(Kalman)
  136.     q_bias_X        += K_1_1 * angle_errX; //(Kalman)


  137.     const float           angle_mY = atan2( ay_m, az_m );  // y axis
  138.     const float           angle_errY = angle_mY - angleY;  // y axis
  139.     const float           C_0_2 = 1;
  140.     const float           PCt_0_2 = C_0_2 * P2[0][0];
  141.     const float           PCt_1_2 = C_0_2 * P2[1][0];
  142.     const float           E_2 =R_angle+ C_0_2 * PCt_0_2;
  143.     const float           K_0_2 = PCt_0_2 / E_2;
  144.     const float           K_1_2 = PCt_1_2 / E_2;
  145.     const float           t_0_2 = PCt_0_2;
  146.     const float           t_1_2 = C_0_2 * P2[0][1];

  147.     P2[0][0] -= K_0_2 * t_0_2;
  148.     P2[0][1] -= K_0_2 * t_1_2;
  149.     P2[1][0] -= K_1_2 * t_0_2;
  150.     P2[1][1] -= K_1_2 * t_1_2;
  151.     angleY += K_0_2 * angle_errY;
  152.     q_bias_Y += K_1_2 * angle_errY;

  153.     const float           angle_mZ = averageZ/5;
  154.     const float           angle_errZ = angle_mZ - angleZ;
  155.     const float           C_0_3 = 1;
  156.     const float           PCt_0_3 = C_0_3 * P3[0][0];
  157.     const float           PCt_1_3 = C_0_3 * P3[1][0];
  158.     const float           E_3 =R_angle+ C_0_3 * PCt_0_3;
  159.     const float           K_0_3 = PCt_0_3 / E_3;
  160.     const float           K_1_3 = PCt_1_3 / E_3;
  161.     const float           t_0_3 = PCt_0_3;
  162.     const float           t_1_3 = C_0_3 * P3[0][1];

  163.     P3[0][0] -= K_0_3 * t_0_3;
  164.     P3[0][1] -= K_0_3 * t_1_3;
  165.     P3[1][0] -= K_1_3 * t_0_3;
  166.     P3[1][1] -= K_1_3 * t_1_3;
  167.     angleZ += K_0_3 * angle_errZ;
  168.     q_bias_Z += K_1_3 * angle_errZ;

  169.     rotationZ = angleZ;

  170.     Serial.print("average z ");Serial.print(averageZ);Serial.print("  lastaverage z ");Serial.println(lastaverageZ);
  171.     Serial.print("  angle_mZ = "); Serial.println(int(angle_mZ * 57.2957795130823));
  172.     Serial.print("Rate  ");Serial.print(int(rate*57.2957795130823)); Serial.print("  Angle X  "); Serial.print(int(angleX*57.2957795130823)); Serial.print("  Angle Y  "); Serial.println(int(angleY*57.2957795130823)); //Prints degrees Acceleromter+Gyros+KalmanFilters
  173.     Serial.print(joy_y_axis); //Prints the adjust for accelerometer jitter
  174.     Serial.print(" ");
  175.     Serial.print(int(angle*57.2957795130823)); //Prints degrees Acceleromter+Gyros+KalmanFilters
  176.     Serial.print(" ");
  177.     Serial.print(int(angle_m*57.295779513082)); //Prints the accelometer
  178.     Serial.print(" ");
  179.     Serial.println(joy_x_axis); //Prints the Gyro adjusment
  180.   }
  181. }
  182. void print ()//This is the function to get the bytes from nintendo wii nunchuck
  183. {
  184.   joy_x_axis = outbuf[0];
  185.   joy_y_axis = outbuf[1];
  186.   ax_m = (outbuf[2] * 2 * 2) -511; //Axis X Acceleromter
  187.   ay_m = (outbuf[3] * 2 * 2) -511; //Axis Y Acceleromter
  188.   az_m = (outbuf[4] * 2 * 2) -511; //Axis Z Acceleromter

  189.   // byte outbuf[5] contains bits for z and c buttons
  190.   // it also contains the least significant bits for the accelerometer data
  191.   // so we have to check each bit of byte outbuf[5]


  192.   if ((outbuf[5] >> 2) & 1)
  193.   {
  194.     ax_m += 2;
  195.   }
  196.   if ((outbuf[5] >> 3) & 1)
  197.   {
  198.     ax_m += 1;
  199.   }

  200.   if ((outbuf[5] >> 4) & 1)
  201.    {
  202.    ay_m += 2;
  203.    }
  204.    if ((outbuf[5] >> 5) & 1)
  205.    {
  206.    ay_m += 1;
  207.    }

  208.   if ((outbuf[5] >> 6) & 1)
  209.   {
  210.     az_m += 2;
  211.   }
  212.   if ((outbuf[5] >> 7) & 1)
  213.   {
  214.     az_m += 1;
  215.   }


  216. }
複製代碼
//程式結束

這個程式很好用,直接copy編譯就可以用,我實驗過了,效果真的很好,
訊號不會亂飄了~ 新手請安心服用嚕~
nichal 該用戶已被刪除
發表於 2012-9-10 11:15:23 | 顯示全部樓層
辛苦了~~~
練功果然是要花時間的!!
發表於 2012-9-10 11:57:40 | 顯示全部樓層
超棒的分享,此篇已置頂並加入精華區
 樓主| 發表於 2012-9-10 14:45:25 | 顯示全部樓層
本帖最後由 tommylin 於 2012-9-10 16:31 編輯

實在不好意思, 野人獻曝了..
To: nichal .. 因為興趣和現實要兼顧阿...呵呵~ 還要請你多多指導喔.
To:娃娃魚版大.. 太謝謝你了,這樣就比較好找帖子了..不會沉下去了 @@

我覺得國外的討論版真正會分享原始程式/硬體的很多,(其實國外網站口水也是很多)
我也只是順手貼到這裡,也會註明原始出處(通常都在程式碼裡面)..
希望年輕人如果因為英文苦手...
這裡可以提供一些線索和方向,
進入正題 ....

PART- 11: 平衡
這個課題估計我要學好幾年了,
試用了一堆板子,讀了一堆文章,最後選定了 MultiWii 來當做教材.
( http://www.multiwii.com )
(超級風行的 UAV 學習課題,學問超級多,範圍超級廣,每個功能都是一門專業的學問)
控制板: MultiWii All In One 主板
( Arduino Mega 2560, MPU6050, HMC5883L,MS5611 )
飛行機構: 泰世-四軸飛行器 330X

MultiWii Mega

MultiWii Mega


為什麼要選擇 MPU6050,希望專家能提供精闢的專業解說...
這個價位是我能負擔且效果最好的了..
但是只要你看過應用 MPU6050的實例,
應該都會認同 C/P值很好吧..@@
也有找到一顆超過 1000美金的,(但是我買不起 = = )
因為我要做的機器人需要不只一顆, 所以..

說明書中提到可以控制的多軸飛行器種類如下:
a.png Avatar.png
b.png
c.png

BiCopter就是俗稱的 阿凡達啦~
發表於 2012-9-10 22:17:44 | 顯示全部樓層
讚喔!!我之前都用ardupilot
現在也改進到APM 2.5版了
能把程式改穩定真是件不容易的事情哩
但現在有個想法
這改用ARM系統來做感覺會更好耶
時脈將進100M ,快了五倍,記憶體也多了很多,IO多很多可以接更多東西,算的更快反應也快
但似乎價格沒有高很多
 樓主| 發表於 2012-9-10 22:44:36 | 顯示全部樓層
本帖最後由 tommylin 於 2012-9-10 22:51 編輯

感謝,鯨魚大大提出建議, 等我先把這個搞定再追品質嚕 ^^

Sensor 讀到信號了,除了 GPS沒時間玩, 現在都到齊了..
本來只要 ACC, GYRO, MAG, AHRS,
現在多了 高度計..
有圖有真相喔 .. (要學的東西好多阿~~ 爽~)
螢幕快照 2012-09-10 下午10.36.34.png
發表於 2012-9-11 14:58:18 | 顯示全部樓層
哇!!!
請問這是直接輸出到遙控上面嗎?
 樓主| 發表於 2012-9-11 15:48:40 | 顯示全部樓層
回復 58# mzw2008

沒啦...只是顯示在 PC端的 GUI.
 樓主| 發表於 2012-9-11 23:50:15 | 顯示全部樓層
因為重要的零件等待從德國飛來,
先來學習一下機構相關的設計..
算是番外篇吧.. 看圖:
IMG_1182.jpg
片身+凹巢然後用鋁柱+螺絲強化結構,組成立體的機身,
看來我機器人的機構也可以自己用CNC這樣製作..
IMG_1185.jpg
加上馬達後,覺得也是很堅固耶~
IMG_1187.JPG
您需要登錄後才可以回帖 登錄 | 申請會員

本版積分規則

小黑屋|手機版|Archiver|機器人論壇 from 2005.07

GMT+8, 2024-3-29 14:27 , Processed in 0.250146 second(s), 9 queries , Apc On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

快速回復 返回頂部 返回列表