Diff: Zlatan-mbed/main.cpp

Revision:

1:110cb8bdfb71

diff -r 4d372883a5b4 -r 110cb8bdfb71 Zlatan-mbed/main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Zlatan-mbed/main.cpp	Wed Jan 04 10:28:53 2017 +0000
@@ -0,0 +1,260 @@
+#include "mbed.h"
+#include "pins.h"
+#include "motor.h"
+#include "definitions.h"
+#include "USBSerial.h"
+
+typedef void (*VoidArray) ();
+
+
+DigitalOut led(LED1);
+DigitalOut l(LED2);
+USBSerial pc;
+
+Ticker motorPidTicker[NUMBER_OF_MOTORS];
+
+bool coilChargeBool = false;
+char buf[16];
+bool serialData = false;
+int serialCount = 0;
+int chargeBool = 0;
+int counter = 0;
+
+volatile int16_t motorTicks[NUMBER_OF_MOTORS];
+volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
+volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
+
+Motor motors[NUMBER_OF_MOTORS];
+
+void serialInterrupt();
+void parseCommand (char *command);
+void parseString (char *str);
+
+void motor0EncTick();
+void motor1EncTick();
+void motor2EncTick();
+
+void motor0PidTick();
+void motor1PidTick();
+void motor2PidTick();
+
+void kickBall();
+void dribblerOn();
+void dribblerOff();
+void coilCharge();
+
+
+int main() {
+    void (*encTicker[])()  = {
+        motor0EncTick,
+        motor1EncTick,
+        motor2EncTick
+    };
+
+    VoidArray pidTicker[] = {
+        motor0PidTick,
+        motor1PidTick,
+        motor2PidTick
+    };
+
+    for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
+        MotorEncA[i]->mode(PullNone);
+        MotorEncB[i]->mode(PullNone);
+
+        motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
+
+        motorTicks[i] = 0;
+        motorEncNow[i] = 0;
+        motorEncLast[i] = 0;
+
+        MotorEncA[i]->rise(encTicker[i]);
+        MotorEncA[i]->fall(encTicker[i]);
+        MotorEncB[i]->rise(encTicker[i]);
+        MotorEncB[i]->fall(encTicker[i]);
+
+        motorPidTicker[i].attach(pidTicker[i], 0.1);
+
+        motors[i].init();
+    }
+
+    
+    KICK = 0;
+    CHARGE = 0;
+    
+    
+    wait_ms(1500);
+    DRIBBLER=70/1000.0;
+    wait_ms(1500);
+    DRIBBLER=10/1000.0;
+    wait_ms(2000);
+    
+    
+    pc.printf("Start\n");
+
+    pc.attach(&serialInterrupt);
+    
+    while(1) {
+                
+        if (serialData) {
+            char temp[32];
+            memcpy(temp, buf, 32);
+            memset(buf, 0, 32);
+            serialData = false;
+            parseString(temp);
+        }
+        if (coilChargeBool) {
+            if ((DONE!=1)){
+                coilChargeBool = false;
+                CHARGE = 0;
+                dribblerOn();
+                pc.printf("CHARGEDD\n");
+            }
+            if (counter>500000){
+                coilChargeBool = false;
+                CHARGE = 0;
+                dribblerOn();
+                pc.printf("CHARGEDT\n");
+            }
+            
+            counter++;
+        }
+        wait_ms(20);
+    }
+}
+
+void serialInterrupt(){
+    while(pc.readable()) {
+        buf[serialCount] = pc.getc();
+        serialCount++;
+    }
+    if (buf[serialCount - 1] == '\n') {
+        serialData = true;
+        serialCount = 0;
+    }
+}
+ 
+void parseString (char *str) {
+    uint8_t x;
+    size_t i = 1;
+    if (str[0] == '<') {
+        char command[300];
+        
+        while (1){
+            x = 0;
+            while(str[i] != ';' && str[i] != '>') {
+                command[x++] = str[i++];
+            }
+            command[x] = '\0';
+            parseCommand(command);
+            if (str[i] == '>') {
+                break;
+            }
+            i++;
+        }
+    }
+}
+
+ 
+ 
+void parseCommand (char *command) {
+ 
+    //MOTOR
+    size_t motor;
+    if (command[0] == '1') motor = 0;
+    else if (command[0] == '2') motor = 1;
+    else motor = 2;
+        
+
+    if (command[0] == 'd' && command[1] == 'o') {
+        int16_t speed = atoi(command + 3);
+        DRIBBLER = speed/1000.0;
+    }
+    
+    if (command[0] == 'd' && command[1] == 'n') {
+        dribblerOn();
+    }
+    
+    if (command[0] == 'd' && command[1] == 'f') {
+        dribblerOff();
+    }
+    
+    if (command[0] == 'c' && command[1] == 'c') {
+        dribblerOff();
+        coilCharge();
+    }
+    
+    if (command[0] == 'c' && command[1] == 'k') {
+        CHARGE = 0;
+        kickBall();
+        dribblerOff();
+        coilCharge();
+    }
+    
+    if (command[0] == 'c' && command[1] == 'e') {
+        CHARGE = 0;
+        kickBall();
+        kickBall();
+        kickBall();
+    }
+    
+    
+    if (command[0] == 'b' && command[1] == 'd') {
+        int n = BALL_DETECT.read();
+        pc.printf("%d\n", n);
+    }
+    
+    if (command[0] == 's') {
+        for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
+            pc.printf("s%d:%d\n", motors[0].getSpeed());
+            pc.printf("s%d:%d\n", i, motors[i].getSpeed());
+        }
+        
+    }
+    if (command[1] == 'w' && command[2] == 'l') {
+        int16_t speed = atoi(command + 3);
+        if (motor==0){
+            motors[0].pid_on = 0;
+            if (speed < 0) motors[0].backward(-1*speed/255.0);
+            else motors[0].forward(speed/255.0);
+        }
+        else{
+            motors[motor].pid_on = 0;
+            if (speed < 0) motors[motor].backward(-1*speed/255.0);
+            else motors[motor].forward(speed/255.0);
+        }
+    } 
+}
+
+void kickBall(){
+    KICK = 1;
+    pc.printf("KICK\n");
+    wait_ms(5);
+    KICK = 0;
+}
+
+void coilCharge(){
+    KICK = 0;
+    CHARGE = 1;
+    counter = 0;
+    coilChargeBool = true;
+    pc.printf("Charge-in\n");
+}
+
+
+void dribblerOn(){
+    DRIBBLER = 58/1000.0;
+}    
+
+void dribblerOff(){
+    DRIBBLER = 51/1000.0;
+}
+
+
+MOTOR_ENC_TICK(0)
+MOTOR_ENC_TICK(1)
+MOTOR_ENC_TICK(2)
+
+
+MOTOR_PID_TICK(0)
+MOTOR_PID_TICK(1)
+MOTOR_PID_TICK(2)
\ No newline at end of file