File content as of revision 3:ff1267cf6b03:

#include "mbed.h"
#include "pins.h"
#include "motor.h"
#include "definitions.h"
#include "USBSerial.h"

typedef void (*VoidArray) ();

DigitalOut led(P2_8);
DigitalOut led2g(P1_18);
//DigitalOut l(P1_18);
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
DigitalIn infrared(P3_25); //PWM - GPIO2
USBSerial pc;

Ticker motorPidTicker[NUMBER_OF_MOTORS];

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];
volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];

Motor motors[NUMBER_OF_MOTORS];

void serialInterrupt();
void parseCommad(char *command);

void motor0EncTick();
void motor1EncTick();
void motor2EncTick();
#if NUMBER_OF_MOTORS == 4
void motor3EncTick();
#endif

void motor0PidTick();
void motor1PidTick();
void motor2PidTick();
#if NUMBER_OF_MOTORS == 4
void motor3PidTick();
#endif

int main() {
    
    void (*encTicker[])()  = {
        motor0EncTick,
        motor1EncTick,
        motor2EncTick,
        #if NUMBER_OF_MOTORS == 4
        motor3EncTick
        #endif
    };

    VoidArray pidTicker[] = {
        motor0PidTick,
        motor1PidTick,
        motor2PidTick,
        #if NUMBER_OF_MOTORS == 4
        motor3PidTick
        #endif
    };

    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();
    }

    //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');
    
    dribbler.period_ms(2);
    while(1) {
        /*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);
            memset(buf, 0, 16);
            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();
        serialCount++;
    }
    if (buf[serialCount - 1] == '\n') {
        serialData = true;
        serialCount = 0;
    }
}

void parseCommad (char *command) {
    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 (command[0] == 'm' && command[1] == 'f') {
        int16_t speed = atoi(command + 2);
        motors[0].pid_on = 1;
        motors[1].pid_on = 1;
        motors[0].setSpeed(speed*-1);
        motors[1].setSpeed(speed);
    }
    if (command[0] == 't') {
        int16_t speed = atoi(command + 1);
        motors[2].pid_on = 1;
        motors[2].setSpeed(speed);
    }
    // l+002r-112b+502
    else if (command[0] == 'l') 
    {
        int16_t lspeed = atoi(command + 1);  
        motors[0].pid_on = 1;
        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(bspeed);           
    }
    else if (command[0] == 'c' && command[1] == 's' && command[2] == 'd') {
        int16_t speed = atoi(command + 3);
        motors[0].pid_on = 1;
        motors[1].pid_on = 1;
        motors[2].pid_on = 1;
        motors[0].setSpeed(0);
        motors[1].setSpeed(speed * -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] == '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)
MOTOR_ENC_TICK(1)
MOTOR_ENC_TICK(2)
#if NUMBER_OF_MOTORS == 4
MOTOR_ENC_TICK(3)
#endif

MOTOR_PID_TICK(0)
MOTOR_PID_TICK(1)
MOTOR_PID_TICK(2)
#if NUMBER_OF_MOTORS == 4
MOTOR_PID_TICK(3)
#endif