Diff: main.cpp

Revision:

2:2a47186e72c3

Parent:

0:da5127da2ba0

Child:

3:ff1267cf6b03

--- a/main.cpp	Thu Oct 13 15:26:32 2016 +0000
+++ b/main.cpp	Fri Nov 11 22:13:25 2016 +0000
@@ -6,8 +6,14 @@
 
 typedef void (*VoidArray) ();
 
-DigitalOut led(LED1);
-DigitalOut l(LED2);
+DigitalOut led(P2_8);
+DigitalOut led2g(P1_18);
+//DigitalOut l(P1_18);
+PwmOut dribbler(P2_4);      //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
+DigitalIn infrared(P3_25); //PWM - GPIO2
 USBSerial pc;
 
 Ticker motorPidTicker[NUMBER_OF_MOTORS];
@@ -40,6 +46,7 @@
 #endif
 
 int main() {
+    
     void (*encTicker[])()  = {
         motor0EncTick,
         motor1EncTick,
@@ -78,16 +85,39 @@
         motors[i].init();
     }
 
-    pc.printf("Start\n");
+    //pc.printf("Start\n");
+    /*motors[0].setSpeed(100);
+    motors[1].setSpeed(50);
+    motors[2].setSpeed(150);*/
+    
+    /*while (pc.readable())
+    {
+        pc.printf("readdddd");
+    }*/
 
     pc.attach(&serialInterrupt);
     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;*/
+    
+    //pc.printf('true');
     while(1) {
-        if (count % 20 == 0) {
+        /*if (count % 20 == 0) {
             for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
                 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
             }
-        }
+        }*/
         if (serialData) {
             char temp[16];
             memcpy(temp, buf, 16);
@@ -95,16 +125,31 @@
             serialData = false;
             parseCommad(temp);
         }
+        if (infrared)
+        {
+            led2g = 0;
+        }
+        else {led2g = 1;}
+        //else {pc.printf("false");}
         //motors[0].pid(motor0Ticks);
         //motor0Ticks = 0;
         wait_ms(50);
         count++;
+        /*while (pc.writeable())
+        {
+            pc.printf("test_write \n");
+        }
+        
+        while (pc.readable())
+        {
+            pc.printf("test_read \n");
+        }*/
 
         //pc.printf("buf: %s\n", buf);
         //pc.printf("Loop\n");
     }
 }
-
+//uint buf[128];
 void serialInterrupt(){
     while(pc.readable()) {
         buf[serialCount] = pc.getc();
@@ -117,11 +162,111 @@
 }
 
 void parseCommad (char *command) {
-    if (command[0] == 's' && command[1] == 'd') {
-        int16_t speed = atoi(command + 2);
+    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 (speedd == 1)
+    {
+        dribbler.pulsewidth_us(135);
+    }*/
+    
+     
+    /*if (command[0] == 's' && command[1] == 'd' && command[2] == '0') {
+        int16_t speed = atoi(command + 3);
         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[1].setSpeed(speed);
+    }
+    if (command[0] == 's' && command[1] == 'd' && command[2] == '2') {
+        int16_t speed = atoi(command + 3);
+        motors[2].pid_on = 1;
+        motors[2].setSpeed(speed);
+    }
+    if (command[0] == 'f') {
+        int16_t speed = atoi(command + 1);
+        motors[0].pid_on = 1;
+        motors[0].setSpeed(-speed);
+        motors[2].pid_on = 1;
+        motors[2].setSpeed(speed);
+    }
+    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') {
+        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[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());