main.cpp

00001 #include "mbed.h"
00002 #include "pins.h"
00003 #include "motor.h"
00004 #include "definitions.h"
00005 #include "USBSerial.h"
00006 
00007 typedef void (*VoidArray) ();
00008 
00009 DigitalOut led(P2_8);
00010 DigitalOut led2g(P1_18);
00011 //DigitalOut l(P1_18);
00012 PwmOut dribbler(P2_4);      //Dribbler pwm pin - B1
00013 DigitalOut charge(P0_10);   //Kicker charge pin - A3
00014 DigitalOut kick(P0_11);     //Kicker kick pin - A2
00015 DigitalIn done(P1_29);     // Kicker done pin - A4
00016 DigitalIn infrared(P3_25); //PWM - GPIO2
00017 USBSerial pc;
00018 
00019 Ticker motorPidTicker[NUMBER_OF_MOTORS];
00020 
00021 char buf[16];
00022 bool serialData = false;
00023 int serialCount = 0;
00024 
00025 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
00026 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
00027 volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
00028 
00029 Motor motors[NUMBER_OF_MOTORS];
00030 
00031 void serialInterrupt();
00032 void parseCommad(char *command);
00033 
00034 void motor0EncTick();
00035 void motor1EncTick();
00036 void motor2EncTick();
00037 #if NUMBER_OF_MOTORS == 4
00038 void motor3EncTick();
00039 #endif
00040 
00041 void motor0PidTick();
00042 void motor1PidTick();
00043 void motor2PidTick();
00044 #if NUMBER_OF_MOTORS == 4
00045 void motor3PidTick();
00046 #endif
00047 
00048 int main() {
00049     
00050     void (*encTicker[])()  = {
00051         motor0EncTick,
00052         motor1EncTick,
00053         motor2EncTick,
00054         #if NUMBER_OF_MOTORS == 4
00055         motor3EncTick
00056         #endif
00057     };
00058 
00059     VoidArray pidTicker[] = {
00060         motor0PidTick,
00061         motor1PidTick,
00062         motor2PidTick,
00063         #if NUMBER_OF_MOTORS == 4
00064         motor3PidTick
00065         #endif
00066     };
00067 
00068     for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00069         MotorEncA[i]->mode(PullNone);
00070         MotorEncB[i]->mode(PullNone);
00071 
00072         motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
00073 
00074         motorTicks[i] = 0;
00075         motorEncNow[i] = 0;
00076         motorEncLast[i] = 0;
00077 
00078         MotorEncA[i]->rise(encTicker[i]);
00079         MotorEncA[i]->fall(encTicker[i]);
00080         MotorEncB[i]->rise(encTicker[i]);
00081         MotorEncB[i]->fall(encTicker[i]);
00082 
00083         motorPidTicker[i].attach(pidTicker[i], 0.1);
00084 
00085         motors[i].init();
00086     }
00087 
00088     //pc.printf("Start\n");
00089     /*motors[0].setSpeed(100);
00090     motors[1].setSpeed(50);
00091     motors[2].setSpeed(150);*/
00092     
00093     /*while (pc.readable())
00094     {
00095         pc.printf("readdddd");
00096     }*/
00097 
00098     pc.attach(&serialInterrupt);
00099     int count = 0;
00100     
00101     
00102     /*dribbler.pulsewidth_us(100);
00103     wait(3.0);
00104     dribbler.pulsewidth_us(140);
00105     wait(5.0);
00106     charge = 1;
00107     wait(8.0);
00108     charge = 0;
00109     wait(1);
00110     kick = 1;
00111     wait_ms(200);
00112     kick = 0;*/
00113     
00114     //pc.printf('true');
00115     while(1) {
00116         /*if (count % 20 == 0) {
00117             for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00118                 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
00119             }
00120         }*/
00121         if (serialData) {
00122             char temp[16];
00123             memcpy(temp, buf, 16);
00124             memset(buf, 0, 16);
00125             serialData = false;
00126             parseCommad(temp);
00127         }
00128         if (infrared)
00129         {
00130             led2g = 0;
00131         }
00132         else {led2g = 1;}
00133         //else {pc.printf("false");}
00134         //motors[0].pid(motor0Ticks);
00135         //motor0Ticks = 0;
00136         wait_ms(50);
00137         count++;
00138         /*while (pc.writeable())
00139         {
00140             pc.printf("test_write \n");
00141         }
00142         
00143         while (pc.readable())
00144         {
00145             pc.printf("test_read \n");
00146         }*/
00147 
00148         //pc.printf("buf: %s\n", buf);
00149         //pc.printf("Loop\n");
00150     }
00151 }
00152 //uint buf[128];
00153 void serialInterrupt(){
00154     while(pc.readable()) {
00155         buf[serialCount] = pc.getc();
00156         serialCount++;
00157     }
00158     if (buf[serialCount - 1] == '\n') {
00159         serialData = true;
00160         serialCount = 0;
00161     }
00162 }
00163 
00164 void parseCommad (char *command) {
00165     char *searcha = command;
00166     char *a;
00167     int indexa;
00168     a=strchr(searcha, 'a');
00169     indexa = int(a - searcha);
00170     
00171     char *searchb = command;
00172     char *b;
00173     int indexb;
00174     b=strchr(searchb, 'b');
00175     indexb = int(b - searchb);
00176     
00177     //dribbler.period(0.058f);
00178     
00179     char *searchc = command;
00180     char *c;
00181     int indexc;
00182     c=strchr(searchc, 'c');
00183     indexc = int(c - searchc);
00184     
00185     /*char *searchd = command;
00186     char *d;
00187     int indexd;
00188     d=strchr(searchd, 'd');
00189     indexd = int(d - searchd);*/
00190     
00191     int16_t speeda = atoi(command + (indexa+1));
00192     motors[0].pid_on = 1;
00193     motors[0].setSpeed(speeda);
00194     
00195     int16_t speedb = atoi(command + (indexb+1));
00196     motors[1].pid_on = 1;
00197     motors[1].setSpeed(speedb);
00198     
00199     int16_t speedc = atoi(command + (indexc+1));
00200     motors[2].pid_on = 1;
00201     motors[2].setSpeed(speedc);
00202     
00203     /*int16_t speedd = atoi(command + (indexd+1));
00204     if (speedd == 0)
00205     {
00206         dribbler.pulsewidth_us(100);
00207         wait(3.0);
00208     }
00209     if (speedd == 1)
00210     {
00211         dribbler.pulsewidth_us(135);
00212     }*/
00213     
00214      
00215     /*if (command[0] == 's' && command[1] == 'd' && command[2] == '0') {
00216         int16_t speed = atoi(command + 3);
00217         motors[0].pid_on = 1;
00218         motors[0].setSpeed(speed);
00219     }
00220     if (command[0] == 's' && command[1] == 'd' && command[2] == '1') {
00221         int16_t speed = atoi(command + 3);
00222         motors[1].pid_on = 1;
00223         motors[1].setSpeed(speed);
00224     }
00225     if (command[0] == 's' && command[1] == 'd' && command[2] == '2') {
00226         int16_t speed = atoi(command + 3);
00227         motors[2].pid_on = 1;
00228         motors[2].setSpeed(speed);
00229     }
00230     if (command[0] == 'f') {
00231         int16_t speed = atoi(command + 1);
00232         motors[0].pid_on = 1;
00233         motors[0].setSpeed(-speed);
00234         motors[2].pid_on = 1;
00235         motors[2].setSpeed(speed);
00236     }
00237     if (command[0] == 'b' && command[1] == '1' && command[2] == '2') {
00238         int16_t speed = atoi(command + 3);
00239         motors[1].pid_on = 1;
00240         motors[1].setSpeed(speed);
00241         motors[2].pid_on = 1;
00242         motors[2].setSpeed(speed);
00243     }
00244         
00245     if (command[0] == 'b' && command[1] == '0' && command[2] == '1') {
00246         int16_t speed = atoi(command + 3);
00247         motors[0].pid_on = 1;
00248         motors[0].setSpeed(speed);
00249         motors[1].pid_on = 1;
00250         motors[1].setSpeed(speed);
00251      }   
00252     if (command[0] == 'b' && command[1] == '0' && command[2] == '2') {
00253         int16_t speed = atoi(command + 3);
00254         motors[0].pid_on = 1;
00255         motors[0].setSpeed(speed);
00256         motors[2].pid_on = 1;
00257         motors[2].setSpeed(speed);
00258       }  
00259     if (command[0] == 'a') {
00260         int16_t speed = atoi(command + 1);
00261         motors[0].pid_on = 1;
00262         motors[0].setSpeed(speed);
00263         motors[1].pid_on = 1;
00264         motors[1].setSpeed(speed);
00265         motors[2].pid_on = 1;
00266         motors[2].setSpeed(speed);
00267     } */
00268 
00269         
00270     if (command[0] == 's') {
00271         for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00272             pc.printf("s%d:%d\n", i, motors[i].getSpeed());
00273         }
00274     } else if (command[0] == 'w' && command[1] == 'l') {
00275         int16_t speed = atoi(command + 2);
00276         motors[0].pid_on = 0;
00277         if (speed < 0) motors[0].backward(-1*speed/255.0);
00278         else motors[0].forward(speed/255.0);
00279     } else if (command[0] == 'p' && command[1] == 'p') {
00280         uint8_t pGain = atoi(command + 2);
00281         motors[0].pgain = pGain;
00282     } else if (command[0] == 'p' && command[1] == 'i') {
00283         uint8_t iGain = atoi(command + 2);
00284         motors[0].igain = iGain;
00285     } else if (command[0] == 'p' && command[1] == 'd') {
00286         uint8_t dGain = atoi(command + 2);
00287         motors[0].dgain = dGain;
00288     } else if (command[0] == 'p') {
00289         char gain[20];
00290         motors[0].getPIDGain(gain);
00291         pc.printf("%s\n", gain);
00292     }
00293 }
00294 
00295 MOTOR_ENC_TICK(0)
00296 MOTOR_ENC_TICK(1)
00297 MOTOR_ENC_TICK(2)
00298 #if NUMBER_OF_MOTORS == 4
00299 MOTOR_ENC_TICK(3)
00300 #endif
00301 
00302 MOTOR_PID_TICK(0)
00303 MOTOR_PID_TICK(1)
00304 MOTOR_PID_TICK(2)
00305 #if NUMBER_OF_MOTORS == 4
00306 MOTOR_PID_TICK(3)
00307 #endif