File content as of revision 4:abf0070897ff:

#include "mbed.h"

//Serial pc(SERIAL_TX, SERIAL_RX);


//Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
AnalogIn ANALOG0(A0);
AnalogIn ANALOG1(A1);
AnalogIn ANALOG2(A2);
AnalogIn ANALOG3(A3);
AnalogIn ANALOG4(A4);
AnalogIn ANALOG5(A5);
SPISlave rpi(PB_5, PB_4, PB_3, PA_15); //setup SPI pins to talk to the raspberry pi
//PA_9 - MLB //PA_8 - MLF //PA_10 - MRF //PA_11 - MRB
PwmOut      MRB(PA_11); //back right motor
PwmOut      MRF(PA_10); //front right motor
PwmOut      MLB(PA_9); //back left motor
PwmOut      MLF(PA_8); //front left motor
DigitalOut  RAIN1(PC_6); //PC_9 - Left //PC_6 - Right
DigitalOut  RAIN2(PB_9); // PC_8 - Left //PB_9 - Right
DigitalOut  RSTANDBY(PC_5); //PB_8 - Left //PC_5 - Right
DigitalOut  LAIN1(PC_9);
DigitalOut  LAIN2(PC_8);
DigitalOut  LSTANDBY(PB_8);
//InterruptIn MRB_ENC1(PB_6);
//InterruptIn MRB_ENC2(PC_7);
InterruptIn MRF_ENC1(PA_13);
InterruptIn MRF_ENC2(PB_7);
//InterruptIn MLB_ENC1(PD_2);
//InterruptIn MLB_ENC2(PC_12);
InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
Ticker      control_timer;

bool        control_loop_flag = false;

//motor associations within arrays
//Right = 0; Left = 1
int m_count [2] = {0,0}; //setup array for 4 encoder counts
int m_speed_ref [2] = {0,0}; //setup array for 4 motor speeds
float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
int m_distance_ref [2] = {0,0}; //setup array for 4 motor distances
int max_accel = 0; //for storing the maximum acceleration
int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values

void readADC(); //read 6 channels of ADC
void setDuty(); //set the 4 motor PWM duty cycles
void setPeriod(int period); //set PWM period for motors in microseconds
void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
void spiComms(); //do spi communications with raspberry pi

void mrfEncoderIsr1();
void mrfEncoderIsr2();
void mlfEncoderIsr1();
void mlfEncoderIsr2();


int main()
{
    float right_speed = 0;
    float left_speed = 1;
    //setup interrupts for encoders
    MRF_ENC1.fall(&mrfEncoderIsr1);
    MRF_ENC2.fall(&mrfEncoderIsr2);
    MLF_ENC1.fall(&mlfEncoderIsr1);
    MLF_ENC2.fall(&mlfEncoderIsr2);
    
    //setup driver pins
    setLMotorDir(1);
    setRMotorDir(1);
    // Set PWM period in us
    setPeriod(100);
 //   pc.printf("Starting up");
    
    while (1) {
 //       pc.printf("Motor count %i\r\n", m_count[0]);
        //write motor speed
        
        right_speed = float(m_speed_ref[0])/100.0;
        if(right_speed < 0){
            setRMotorDir(0); //set the motors backwards
            right_speed = -1*right_speed; //make the negative value into positive
        } else {
            setRMotorDir(1); //set the motors forwards
        }
        if(right_speed > 100){ 
            right_speed = 100; //make sure the speed isn't greater than 100%
        }
        m_duty[0] = right_speed;
        
        left_speed = float(m_speed_ref[1])/100.0;
        if(left_speed < 0){
            setLMotorDir(0); //set the motors backwards
            left_speed = -1*left_speed; //make the negative value into positive
        } else {
            setLMotorDir(1); //set the motors forwards
        }
        if(left_speed > 100){ 
            left_speed = 100; //make sure the speed isn't greater than 100%
        }
        m_duty[1] = left_speed;
               
        setDuty(); //set all the duty cycles to the motor drivers
        readADC(); //read all the ADC values when not doing something else
        spiComms(); //do SPI communication stuff
    }
}

//function to read all 6 ADC channels
void readADC(){
    adc_values[0] = int(ANALOG0.read()*255);
    adc_values[1] = int(ANALOG1.read()*255);
    adc_values[2] = int(ANALOG2.read()*255);
    adc_values[3] = int(ANALOG3.read()*255);
    adc_values[4] = int(ANALOG4.read()*255);
    adc_values[5] = int(ANALOG5.read()*255);
}

//function to set all 4 motor PWM duty cycles
void setDuty(){
    MRB.write(m_duty[0]);
    MRF.write(m_duty[0]);
    MLB.write(m_duty[1]);
    MLF.write(m_duty[1]);
}
//set left motor direction. 1 is forward, 0 is backwards.
void setLMotorDir(bool direction){
    LSTANDBY = 1;
    if(direction == true){
        LAIN1 = 1;
        LAIN2 = 0;
    }
    else if(direction == false){
        LAIN1 = 0;
        LAIN2 = 1;
    }
}
//set right motor direction. 1 is forward, 0 is backwards.
void setRMotorDir(bool direction){
    RSTANDBY = 1;
    if(direction == true){
        RAIN1 = 0;
        RAIN2 = 1;
    }
    else if(direction == false){
        RAIN1 = 1;
        RAIN2 = 0;
    }
}

//initialisation function to set motor PWM period and set to 0 duty
void setPeriod(int period){
    MRB.period_us(period);
    MRB.write(0.0);
    MRF.period_us(period);
    MRF.write(0.0);
    MLB.period_us(period);
    MLB.write(0.0);
    MLF.period_us(period);
    MLF.write(0.0);
}

void spiComms(){
       //do SPI communication stuff
    int i = 0; //temp counter variable
    int v = 0; //temp SPI variable
    if(rpi.receive()) {
        v = rpi.read();   // Read byte from master
        if(v == char('S')){
            rpi.reply(0x01);
            for (i=0;i<=1;i++){                   
                m_speed_ref[i] = rpi.read() - 128;
                rpi.reply(m_speed_ref[i]);
            }
            v = rpi.read(); //last bit setup a blank reply
            rpi.reply(0x00);
        }
        else if(v == char('D')){
            rpi.reply(0x02);
            for (i=0;i<=1;i++){                   
                m_speed_ref[i] = rpi.read() - 128;
                rpi.reply(m_speed_ref[i]);
            }
            for (i=0;i<=1;i++){                   
                m_distance_ref[i] = rpi.read() - 128;
                rpi.reply(m_distance_ref[i]);
            }
            v = rpi.read(); //last bit setup a blank reply
            rpi.reply(0x00);
        }
        else if(v == char('A')){
            rpi.reply(0x03);                 
            max_accel = rpi.read();
            rpi.reply(max_accel);
            v = rpi.read(); //last bit setup a blank reply
            rpi.reply(0x00);
        }
        else if(v == char('V')){
            rpi.reply(0x04);                 
            v = rpi.read();
            if(v <= 6){ //check the ADC to be addressed exists
                rpi.reply(adc_values[v]);
            }
            v = rpi.read(); //last bit setup a blank reply
            rpi.reply(0x00);
        }
    }
}
void mrfEncoderIsr1(){
    if(MRF_ENC2 == 0) { 
        m_count[0] ++;
    } else {
        m_count[0] --;
    }
}
void mrfEncoderIsr2(){
    if(MRF_ENC1 == 1) {
        m_count[0] ++;
    } else {
        m_count[0] --;
    }
}
void mlfEncoderIsr1(){
    if(MLF_ENC2 == 0) {
        m_count[1] ++;
    } else {
        m_count[1] --;
    }
}
void mlfEncoderIsr2(){
    if(MLF_ENC1 == 1) {
        m_count[1] ++;
    } else {
        m_count[1] --;
    }
}