Revision 3:be341df4265f, committed 2016-11-14

Comitter:

Mihkelval

Date:

Mon Nov 14 19:43:02 2016 +0000

Parent:

2:31a7326e182e

Commit message:

asd

Changed in this revision

--- a/main.cpp	Thu Nov 10 13:42:20 2016 +0000
+++ b/main.cpp	Mon Nov 14 19:43:02 2016 +0000
@@ -50,7 +50,7 @@
 void motor3PidTick();
 #endif
 
-
+int m = 1;
 
 
 int main() {
@@ -97,7 +97,6 @@
     int count = 0;
     dribbler.period_ms(20);
 
-
     while(1) {
        //if (Done.read() == 1){
        //     Charge.write(0);
@@ -121,7 +120,7 @@
         }
         //motors[0].pid(motor0Ticks);
         //motor0Ticks = 0;
-        wait_ms(5);
+        wait_ms(1);
         count++;
         //pc.printf("buf: %s\n", buf);
         //pc.printf("Loop\n");
@@ -171,54 +170,7 @@
         for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
             pc.printf("s%d:%d\n", i, motors[i].getSpeed());
         }
-    } else if (command[0] == 'a' && command[1] == 'w' && command[2] == 'l') {
-        int16_t speed = atoi(command + 3);
-        motors[0].pid_on = 0;
-        motors[1].pid_on = 0;
-        motors[2].pid_on = 0;
-        if (speed < 0) {
-            motors[0].backward(-1*speed/255.0);
-            motors[1].backward(-1*speed/255.0);
-            motors[2].backward(0);
-            }
-        else {
-            motors[0].forward(speed/255.0);
-            motors[1].forward(speed/255.0);
-            motors[2].forward(0);
-            }
-    }
-    else if (command[0] == 'b' && command[1] == 'w' && command[2] == 'l') {
-        int16_t speed = atoi(command + 3);
-        motors[0].pid_on = 0;
-        motors[1].pid_on = 0;
-        motors[2].pid_on = 0;
-        if (speed < 0) {
-            motors[0].backward(-1*speed/255.0);
-            motors[1].backward(0);
-            motors[2].backward(-1*speed/255.0);
-            }
-        else {
-            motors[0].forward(speed/255.0);
-            motors[1].forward(0);
-            motors[2].forward(speed/255.0);
-            }
-    }
-    else if (command[0] == 'c' && command[1] == 'w' && command[2] == 'l') {
-        int16_t speed = atoi(command + 3);
-        motors[0].pid_on = 0;
-        motors[1].pid_on = 0;
-        motors[2].pid_on = 0;
-        if (speed < 0) {
-            motors[0].backward(0);
-            motors[1].backward(-1*speed/255.0);
-            motors[2].backward(-1*speed/255.0);
-            }
-        else {
-            motors[0].forward(0);
-            motors[1].forward(speed/255.0);
-            motors[2].forward(speed/255.0);
-            }
-    }
+        }
      else if (command[0] == 'p' && command[1] == 'p') {
         uint8_t pGain = atoi(command + 2);
         motors[0].pgain = pGain;
@@ -240,9 +192,8 @@
         pc.printf("%s\n", gain);
     }
     else if (command[0] == 'd') {
-        float PWM = atof(command+1);
-        PWM = PWM /100000;
-        dribbler.write(PWM);
+        int PWM = atoi(command+1);
+        dribbler.pulsewidth_us(PWM);
         }
     else if (command[0] == 'j') {
         Charge.write(1);
@@ -251,10 +202,15 @@
         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)