Diff: main.cpp

Revision:

3:ff1267cf6b03

Parent:

2:2a47186e72c3

--- a/main.cpp	Fri Nov 11 22:13:25 2016 +0000
+++ b/main.cpp	Mon Nov 28 17:19:57 2016 +0000
@@ -9,7 +9,7 @@
 DigitalOut led(P2_8);
 DigitalOut led2g(P1_18);
 //DigitalOut l(P1_18);
-PwmOut dribbler(P2_4);      //Dribbler pwm pin - B1
+PwmOut dribbler(P2_5);      //Dribbler pwm pin - B1
 DigitalOut charge(P0_10);   //Kicker charge pin - A3
 DigitalOut kick(P0_11);     //Kicker kick pin - A2
 DigitalIn done(P1_29);     // Kicker done pin - A4
@@ -21,6 +21,7 @@
 char buf[16];
 bool serialData = false;
 int serialCount = 0;
+int m=1;
 
 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
@@ -99,19 +100,21 @@
     int count = 0;
     
     
-    /*dribbler.pulsewidth_us(100);
-    wait(3.0);
-    dribbler.pulsewidth_us(140);
-    wait(5.0);
-    charge = 1;
-    wait(8.0);
-    charge = 0;
-    wait(1);
-    kick = 1;
-    wait_ms(200);
-    kick = 0;*/
+    //dribbler.pulsewidth_us(100);
+//    wait(3.0);
+//    dribbler.pulsewidth_us(140);
+//    wait(5.0);
+//    charge = 1;
+//    wait(8.0);
+//    charge = 0;
+//    wait(1);
+//    kick = 1;
+//    wait_ms(200);
+//    kick = 0;
     
     //pc.printf('true');
+    
+    dribbler.period_ms(2);
     while(1) {
         /*if (count % 20 == 0) {
             for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
@@ -162,134 +165,94 @@
 }
 
 void parseCommad (char *command) {
-    char *searcha = command;
-    char *a;
-    int indexa;
-    a=strchr(searcha, 'a');
-    indexa = int(a - searcha);
-    
-    char *searchb = command;
-    char *b;
-    int indexb;
-    b=strchr(searchb, 'b');
-    indexb = int(b - searchb);
-    
-    //dribbler.period(0.058f);
-    
-    char *searchc = command;
-    char *c;
-    int indexc;
-    c=strchr(searchc, 'c');
-    indexc = int(c - searchc);
-    
-    /*char *searchd = command;
-    char *d;
-    int indexd;
-    d=strchr(searchd, 'd');
-    indexd = int(d - searchd);*/
-    
-    int16_t speeda = atoi(command + (indexa+1));
-    motors[0].pid_on = 1;
-    motors[0].setSpeed(speeda);
-    
-    int16_t speedb = atoi(command + (indexb+1));
-    motors[1].pid_on = 1;
-    motors[1].setSpeed(speedb);
-    
-    int16_t speedc = atoi(command + (indexc+1));
-    motors[2].pid_on = 1;
-    motors[2].setSpeed(speedc);
-    
-    /*int16_t speedd = atoi(command + (indexd+1));
-    if (speedd == 0)
-    {
-        dribbler.pulsewidth_us(100);
-        wait(3.0);
+    if (command[0] == 'c' && command[1] == 'a') {
+        int16_t speed = atoi(command + 2);
+        motors[0].pid_on = 1;
+        motors[1].pid_on = 1;
+        motors[2].pid_on = 1;
+        motors[0].setSpeed(speed);
+        motors[1].setSpeed(speed);
+        motors[2].setSpeed(speed);
     }
-    if (speedd == 1)
-    {
-        dribbler.pulsewidth_us(135);
-    }*/
-    
-     
-    /*if (command[0] == 's' && command[1] == 'd' && command[2] == '0') {
-        int16_t speed = atoi(command + 3);
+    if (command[0] == 'm' && command[1] == 'f') {
+        int16_t speed = atoi(command + 2);
         motors[0].pid_on = 1;
-        motors[0].setSpeed(speed);
-    }
-    if (command[0] == 's' && command[1] == 'd' && command[2] == '1') {
-        int16_t speed = atoi(command + 3);
         motors[1].pid_on = 1;
+        motors[0].setSpeed(speed*-1);
         motors[1].setSpeed(speed);
     }
-    if (command[0] == 's' && command[1] == 'd' && command[2] == '2') {
-        int16_t speed = atoi(command + 3);
+    if (command[0] == 't') {
+        int16_t speed = atoi(command + 1);
         motors[2].pid_on = 1;
         motors[2].setSpeed(speed);
     }
-    if (command[0] == 'f') {
-        int16_t speed = atoi(command + 1);
+    // l+002r-112b+502
+    else if (command[0] == 'l') 
+    {
+        int16_t lspeed = atoi(command + 1);  
         motors[0].pid_on = 1;
-        motors[0].setSpeed(-speed);
+        motors[0].setSpeed(lspeed); 
+        int16_t rspeed = atoi(command + 6); 
+        motors[1].pid_on = 1;
+        motors[1].setSpeed(rspeed); 
+        int16_t bspeed = atoi(command + 11); 
         motors[2].pid_on = 1;
-        motors[2].setSpeed(speed);
+        motors[2].setSpeed(bspeed);           
     }
-    if (command[0] == 'b' && command[1] == '1' && command[2] == '2') {
-        int16_t speed = atoi(command + 3);
-        motors[1].pid_on = 1;
-        motors[1].setSpeed(speed);
-        motors[2].pid_on = 1;
-        motors[2].setSpeed(speed);
-    }
-        
-    if (command[0] == 'b' && command[1] == '0' && command[2] == '1') {
+    else if (command[0] == 'c' && command[1] == 's' && command[2] == 'd') {
         int16_t speed = atoi(command + 3);
         motors[0].pid_on = 1;
-        motors[0].setSpeed(speed);
         motors[1].pid_on = 1;
-        motors[1].setSpeed(speed);
-     }   
-    if (command[0] == 'b' && command[1] == '0' && command[2] == '2') {
-        int16_t speed = atoi(command + 3);
-        motors[0].pid_on = 1;
-        motors[0].setSpeed(speed);
         motors[2].pid_on = 1;
+        motors[0].setSpeed(0);
+        motors[1].setSpeed(speed * -1);
         motors[2].setSpeed(speed);
-      }  
-    if (command[0] == 'a') {
-        int16_t speed = atoi(command + 1);
-        motors[0].pid_on = 1;
-        motors[0].setSpeed(speed);
-        motors[1].pid_on = 1;
-        motors[1].setSpeed(speed);
-        motors[2].pid_on = 1;
-        motors[2].setSpeed(speed);
-    } */
-
-        
+    }
     if (command[0] == 's') {
         for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
             pc.printf("s%d:%d\n", i, motors[i].getSpeed());
         }
-    } else if (command[0] == 'w' && command[1] == 'l') {
-        int16_t speed = atoi(command + 2);
-        motors[0].pid_on = 0;
-        if (speed < 0) motors[0].backward(-1*speed/255.0);
-        else motors[0].forward(speed/255.0);
-    } else if (command[0] == 'p' && command[1] == 'p') {
+        }
+     else if (command[0] == 'p' && command[1] == 'p') {
         uint8_t pGain = atoi(command + 2);
         motors[0].pgain = pGain;
+        motors[1].pgain = pGain;
+        motors[2].pgain = pGain;
     } else if (command[0] == 'p' && command[1] == 'i') {
         uint8_t iGain = atoi(command + 2);
         motors[0].igain = iGain;
+        motors[1].igain = iGain;
+        motors[2].igain = iGain;
     } else if (command[0] == 'p' && command[1] == 'd') {
         uint8_t dGain = atoi(command + 2);
         motors[0].dgain = dGain;
+        motors[1].dgain = dGain;
+        motors[2].dgain = dGain;
     } else if (command[0] == 'p') {
         char gain[20];
         motors[0].getPIDGain(gain);
         pc.printf("%s\n", gain);
     }
+    else if (command[0] == 'd') {
+        int PWM = atoi(command+1);
+        dribbler.pulsewidth_us(PWM);
+        }
+    else if (command[0] == 'j') {
+        charge.write(1);
+        }
+    else if (command[0] == 'l') {
+        charge.write(0);
+        wait_ms(1);
+        kick.write(1);
+        wait_ms(m);
+        kick.write(0);
+        }
+    else if (command[0] == 'k'){
+        kick.write(0);
+        }
+    else if (command[0] == 'm'){
+        m = atoi(command+1);
+        }
 }
 
 MOTOR_ENC_TICK(0)