File content as of revision 1:110cb8bdfb71:

#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)