File content as of revision 3:be341df4265f:

#include "mbed.h"
#include "pins.h"
#include "motor.h"
#include "definitions.h"
#include "USBSerial.h"
// mootor 3 liigub wl kasuga aeglasemalt, kui teised mootorid.
typedef void (*VoidArray) ();


//InterruptIn event(P3_25);
DigitalIn enable(P3_25);

DigitalIn Done(P1_29);
DigitalOut Charge(P0_10);
DigitalOut Kick(P0_11);


PwmOut dribbler(P2_5);

DigitalOut led(LED1);
DigitalOut l(LED2);
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 m = 1;


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");

    pc.attach(&serialInterrupt);
    int count = 0;
    dribbler.period_ms(20);

    while(1) {
       //if (Done.read() == 1){
       //     Charge.write(0);
       //     wait_ms(5);
       //     Kick.write(1);    }
        
        if (count % 50 == 0) {
            //for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
            //    pc.printf("s%d:%d\n", i, motors[i].getSpeed());
            //}
            pc.printf("%d\n", enable.read());
            //pc.printf("%d\n", Done.read());
            
        }
        if (serialData) {
            char temp[16];
            memcpy(temp, buf, 16);
            memset(buf, 0, 16);
            serialData = false;
            parseCommad(temp);
        }
        //motors[0].pid(motor0Ticks);
        //motor0Ticks = 0;
        wait_ms(1);
        count++;
        //pc.printf("buf: %s\n", buf);
        //pc.printf("Loop\n");
    }
}

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] == 'a' && 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(speed);
        motors[1].setSpeed(speed);
        motors[2].setSpeed(0);
    }
    else if (command[0] == 'b' && 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(speed);
        motors[1].setSpeed(0);
        motors[2].setSpeed(speed);
    }
    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