File content as of revision 2:2a47186e72c3:

#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_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];

char buf[16];
bool serialData = false;
int serialCount = 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 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');
    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) {
    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());
        }
    } else if (command[0] == 'w' && command[1] == 'l') {
        int16_t speed = atoi(command + 2);
        motors[0].pid_on = 0;
        if (speed < 0) motors[0].backward(-1*speed/255.0);
        else motors[0].forward(speed/255.0);
    } else if (command[0] == 'p' && command[1] == 'p') {
        uint8_t pGain = atoi(command + 2);
        motors[0].pgain = pGain;
    } else if (command[0] == 'p' && command[1] == 'i') {
        uint8_t iGain = atoi(command + 2);
        motors[0].igain = iGain;
    } else if (command[0] == 'p' && command[1] == 'd') {
        uint8_t dGain = atoi(command + 2);
        motors[0].dgain = dGain;
    } else if (command[0] == 'p') {
        char gain[20];
        motors[0].getPIDGain(gain);
        pc.printf("%s\n", gain);
    }
}

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