File content as of revision 5:600c5c9cbb19:

#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 2 encoder counts
int m_last_count [2] = {0,0}; //setup array for storing previous encoder counts
int m_speed_ref [2] = {0,0}; //setup array for 2 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 2 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();
void controlTick();


int main()
{
    float pos_change = 0; //temp variable for speed control
    float speed_error = 0; //temp variable for speed control
    float integral [2] = {0,0};
    float Kp = 0.01; //proportional constant
    float Ki = 0.01; //integral constant
    int i = 0;
    
    
    //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");
    //setup control loop
    control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
    

    
    while (1) {
 //       pc.printf("Motor count %i\r\n", m_count[0]);

        if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
            for(i=0;i<=1;i++){ //do this for right and left in turn
                pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
                m_last_count[i] = m_count[i]; //store count for next cycle
                speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
                integral[i] = integral[i] + speed_error;
                m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
            }
    
            if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
                m_duty[0] = 0;
            if(m_speed_ref[1] == 0)
                m_duty[1] = 0;
                 
            if(m_duty[0] < 0){
                setRMotorDir(0); //set the motors backwards
                m_duty[0] = -1*m_duty[0]; //make the negative value into positive
            } else {
                setRMotorDir(1); //set the motors forwards
            }
            if(m_duty[0] > 100){ 
                m_duty[0] = 100; //make sure the speed isn't greater than 100%
            }
            
            if(m_duty[1] < 0){
                setLMotorDir(0); //set the motors backwards
                m_duty[1] = -1*m_duty[1]; //make the negative value into positive
            } else {
                setLMotorDir(1); //set the motors forwards
            }
            if(m_duty[1] > 100){ 
                m_duty[1] = 100; //make sure the speed isn't greater than 100%
            }                   
            setDuty(); //set all the duty cycles to the motor drivers
            control_loop_flag = false;
        }
        
        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] --;
    }
}

void controlTick()
{
    control_loop_flag = true;
}