main.cpp

00001 #include "mbed.h"
00002 #include "MPU9150.h"
00003 #include "Quaternion.h"
00004 #include "Motor.h"
00005 
00006 
00007 void initCaptures(void);
00008 
00009 //led pins
00010 DigitalOut led1(P2_6);
00011 DigitalOut led2(P2_7);
00012 DigitalOut led3(P2_8);
00013 float P=0,I=0,D=0;
00014 int pulsesMotor2=0,pulsesMotor1=0;
00015 int int_time=0,bint_time=0;
00016 
00017 
00018 Serial RN42(P0_10, P0_11);
00019 Serial debug(P0_2, P0_3);
00020 
00021 Ticker infoTicker,commandTicker,swaveTicker;
00022 
00023 Timer timer;
00024 Timer timer2;
00025 
00026 
00027 MPU9150 imu(P0_28, P0_27, P2_13);
00028 
00029 Motor motor2(P2_2, P2_3, pulsesMotor2);
00030 
00031 Motor motor1(P2_0, P2_1, pulsesMotor1);
00032 AnalogIn sg1(P0_24);
00033 AnalogIn sg2(P0_23);
00034 AnalogIn hallSensor(P0_26);
00035 char buffer[200];
00036 void infoTask(void)
00037 {
00038     led1=!led1;
00039 }
00040 int str2int(char* str)
00041 {
00042     int num = 0, i = 0;
00043     while(str[i] != '\n') {
00044         num = num * 10 + str[i] - '0';
00045         i++;
00046     }
00047     return num;
00048 }
00049 
00050 bool squarewave=0;
00051 void swaveTask(void)
00052 {
00053     if(squarewave) {
00054         motor1.setAngle(5);
00055         motor2.setAngle(5);
00056     } else {
00057         motor1.setAngle(55);
00058         motor2.setAngle(55);
00059     }
00060     squarewave=!squarewave;
00061 }
00062 float pidStep=1000.0f;
00063 void getCommand(void)
00064 {
00065     if(debug.readable()) {
00066         char buffer[128];
00067         debug.gets(buffer, 20);
00068         if(buffer[0]=='m') {
00069             float speed = (float)(str2int(buffer+3));
00070             debug.printf("FLOAT %f FLOAT ",speed);
00071             if(buffer[1]=='1') {
00072                 motor1.setAngle(speed);
00073             }
00074 
00075             if(buffer[1]=='2') {
00076 
00077                 motor2.setAngle(speed);
00078             }
00079         } else if(buffer[0]=='p') {
00080             if(buffer[1]=='+') {
00081                 P+=1/pidStep;
00082             }
00083 
00084             if(buffer[1]=='-') {
00085                 P-=1/pidStep;
00086             }
00087             motor2.setPID(P,I,D);
00088         } else if(buffer[0]=='i') {
00089             if(buffer[1]=='+') {
00090                 I+=1/pidStep;
00091             }
00092             if(buffer[1]=='-') {
00093                 I-=1/pidStep;
00094             }
00095             motor2.setPID(P,I,D);
00096         } else if(buffer[0]=='d') {
00097             if(buffer[1]=='+') {
00098                 D+=1/pidStep;
00099             }
00100 
00101             if(buffer[1]=='-') {
00102                 D-=1/pidStep;
00103             }
00104             motor2.setPID(P,I,D);
00105         } else if(buffer[0]=='s') {
00106             pidStep = (float)(str2int(buffer+2));
00107         }
00108     }
00109 }
00110 int main()
00111 {
00112 
00113     RN42.baud(9600);
00114     debug.baud(115200);
00115     //InitTimer0();
00116     //initialize_connection();
00117     infoTicker.attach(infoTask,0.2f);
00118    // commandTicker.attach(getCommand,0.2f);
00119     if(imu.isReady()) {
00120         debug.printf("MPU9150 is ready\r\n");
00121     } else {
00122         debug.printf("MPU9150 initialisation failure\r\n");
00123     }
00124 
00125     imu.initialiseDMP();
00126 
00127     timer.start();
00128     timer2.start();
00129 
00130     imu.setFifoReset(true);
00131     imu.setDMPEnabled(true);
00132 
00133     Quaternion q1;
00134     motor1.setAngle(-60);
00135 
00136     motor2.setAngle(-60);
00137     wait_ms(1000);
00138     motor1.setAngle(0);
00139 
00140     motor2.setAngle(0);
00141     wait_ms(100);
00142     initCaptures();
00143 
00144     swaveTicker.attach(swaveTask,.20f);
00145 
00146     while(1) {
00147         if(imu.getFifoCount() >= 48) {
00148             imu.getFifoBuffer(buffer,  48);
00149             led2 = !led2;
00150         }
00151 
00152         if(timer.read_ms() > 50) {
00153             timer.reset();
00154 
00155             //This is the format of the data in the fifo,
00156             /* ================================================================================================ *
00157               | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
00158               |                                                                                                  |
00159               | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
00160               |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
00161               |                                                                                                  |
00162               | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
00163               |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
00164               * ================================================================================================ */
00165 
00166 
00167             /*  debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]),
00168                                               (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
00169                                               (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
00170 
00171               debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
00172                                               (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
00173                                               (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
00174 
00175               debug.printf("%d, %d, %d\r\n",  (int16_t)(buffer[29] << 8) + buffer[28],
00176                                               (int16_t)(buffer[31] << 8) + buffer[30],
00177                                               (int16_t)(buffer[33] << 8) + buffer[32]);
00178 
00179               debug.printf("%f, %f, %f, %f\r\n",
00180                   (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
00181                   (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
00182                   (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
00183                   (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
00184             */
00185             q1.decode(buffer);
00186 
00187             debug.printf("%f, %f ", sg1.read(),sg2.read());
00188             debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
00189 
00190            
00191             Vector3 pry=q1.getEulerAngles();
00192             debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
00193            // debug.printf("m1: %.2f m2: %.2f ",motor1.getAngle(),motor2.getAngle());
00194             //debug.printf("dt1: %f dt2: %f ",motor1.get_dt(),motor2.get_dt());
00195             debug.printf("P: %f I: %f I: %f  \r\n ",P,I,D);
00196             getCommand();
00197             /*  if(!motor1.isRunning()) {
00198                   if(motor1.getAngle()>30) {
00199                       motor1.setAngle(0);
00200                   }
00201                   if(motor1.getAngle()<30) {
00202                       motor1.setAngle(60);
00203                   }
00204               }
00205               if(!motor2.isRunning()) {
00206                   if(motor2.getAngle()>30) {
00207                       motor2.setAngle(0);
00208                   }
00209                   if(motor2.getAngle()<30) {
00210                       motor2.setAngle(60);
00211                   }
00212               }*/
00213         }
00214     }
00215 
00216 }
00217 
00218 
00219 
00220 
00221 void TIMER2_IRQHandler(void)
00222 {
00223     //   timer2.reset();
00224     int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
00225     if(bitB) {
00226         pulsesMotor2--;
00227         int_time=0;
00228     } else {
00229         pulsesMotor2++;
00230         int_time=1;
00231     }
00232 
00233     LPC_TIM2->IR |= 0x10;
00234     //bint_time=timer2.read_us();
00235 }
00236 
00237 void TIMER0_IRQHandler(void)
00238 {
00239     int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1;
00240     if(bitB) {
00241         pulsesMotor1--;
00242     } else {
00243         pulsesMotor1++;
00244     }
00245 
00246     LPC_TIM0->IR |= 0x10;
00247 }
00248 
00249 void initCaptures(void)
00250 {
00251     NVIC_SetPriority (TIMER3_IRQn, 6);
00252     LPC_PINCON->PINSEL0  |= 0x00000300; // set P0.4 to CAP2.0
00253     LPC_SC->PCONP |= (1 << 22);         // Timer2 power on
00254     // init Timer 2 (cap2.0)
00255     LPC_TIM2->IR = 0x10;          // clear interrupt register
00256     NVIC_SetVector(TIMER2_IRQn, uint32_t(TIMER2_IRQHandler));
00257     LPC_TIM2->TC = 0;                   // clear timer counter
00258     LPC_TIM2->PC = 0;                   // clear prescale counter
00259     LPC_TIM2->PR = 0;                   // clear prescale register
00260     LPC_TIM2->CCR |= 0x5;       // enable cap2.0 rising capture; interrupt on cap2.0
00261     NVIC_SetPriority (TIMER2_IRQn, 5);
00262     LPC_TIM2->TCR = (1 << 0);          // start Timer2
00263     NVIC_EnableIRQ(TIMER2_IRQn);
00264 
00265     LPC_PINCON->PINSEL3  |= 0x00300000; // set P1.26 to CAP0.0
00266     LPC_SC->PCONP |= (1 << 1);         // Timer0 power on
00267     // init Timer 0 (cap0.0)
00268     LPC_TIM0->IR = 0x10;          // clear interrupt register
00269     NVIC_SetVector(TIMER0_IRQn, uint32_t(TIMER0_IRQHandler));
00270     LPC_TIM0->TC = 0;                   // clear timer counter
00271     LPC_TIM0->PC = 0;                   // clear prescale counter
00272     LPC_TIM0->PR = 0;                   // clear prescale register
00273     LPC_TIM0->CCR |= 0x5;       // enable cap0.0 rising capture; interrupt on cap0.0
00274     NVIC_SetPriority (TIMER0_IRQn, 5);
00275     LPC_TIM0->TCR = (1 << 0);          // start Timer0
00276     NVIC_EnableIRQ(TIMER0_IRQn);
00277 
00278 }