File content as of revision 8:826ec74d53c9:

#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
int m_top_speed [2] = {0,0};
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 = 5; //for storing the maximum acceleration
int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
int distance_scale = 14; 
//max number over SPI is 100. Scale to make this equivalent to 1m 
//with 48mm diameter wheels and 210 counts per revolution. Circumference = pi*D = 0.1508m per rev.
//1m/0.1508m = 6.63. 6.63*210 = 1392 counts. So scale to make 100 = 1400 counts
 
float integral [2] = {0,0};
bool control_mode = true; //control mode flag. True is speed control and False is distance control
bool m_direction [2] = {true, true};
 
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();
float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) ;
 
 
int main()
{
    float pos_change = 0; //temp variable for speed control
    float speed_error = 0; //temp variable for speed control
 
    float Kp = 0.01; //proportional constant
    float Ki = 0.01; //integral constant
    int i = 0;
    
    float distance_error = 0;
    float Kp_d = 0.01;
    float max_duty = 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
            if(control_mode == true) { //speed control
                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;
            }
            else if(control_mode == false) { //distance x and max speed y control
                int targetSpeed=0;
                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
                    targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], m_direction[i]);//m_speed_ref is used as top speed
                    speed_error = pos_change - targetSpeed; //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
                
                
                
                    //distance_error = float(m_count[i]-m_distance_ref[i]); //calculate difference between current speed and reference speed
                    //m_duty[i] = Kp_d*distance_error; //speed proportional control
                    //max_duty = float(m_top_speed[i])/100;
                    //if(m_duty[i] > max_duty){ //check the max duty hasn't been exceeded
                    //    m_duty[i] = max_duty;
                    //}
                    //else if(m_duty[i] < -1*max_duty){
                    //    m_duty[i] = -1*max_duty;
                    //}
                }
    /*            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;
        }
        for(i=0;i<1;i++){
            if(m_distance_ref[i]<0)
                m_direction[i] = false;
            else
                m_direction[i] = true;
        }
        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);
            control_mode = true; //set the controller go do distance control
            m_count[0] = 0; //reset encoder counts to avoid overflow
            m_count[1] = 0;
            integral[0] = 0; //reset integrals to avoid odd behaviour
            integral[1] = 0;
        }
        else if(v == char('D')){
            rpi.reply(0x02);
            for (i=0;i<=1;i++){                   
                m_distance_ref[i] = rpi.read() - 128;
                rpi.reply(m_distance_ref[i]);
                m_distance_ref[i] = m_distance_ref[i] * distance_scale;
            }
            for (i=0;i<=1;i++){                   
                m_top_speed[i] = rpi.read() - 128;
                rpi.reply(m_top_speed[i]);
            }
            v = rpi.read(); //last bit setup a blank reply
            rpi.reply(0x00);
            control_mode = false; //set the controller go do distance control
            m_count[0] = 0; //reset encoder counts to avoid overflow
            m_count[1] = 0;
            m_last_count[0] = 0;
            m_last_count[1] = 0;
        }
        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;
}

        
float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) {
//targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], true);//m_speed_ref is used as top speed
//note to self: check direction
    
 //   end = end/float(distance_scale);
    float minSpeed=1;//deg per sec - to prevent motor speed=0 and therefore jam by delay=infinity.
    float halfWay = 0.0;
    float v = 0.0;
    float midPointVsqrd = 0.0;
    halfWay=start+(end-start)/2;
      
  //  if (direction == true) {
        if (abs(currentPosition)>abs(halfWay)) {
          //deaccelarate
          midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-start);
          v=sqrt(midPointVsqrd-2*accel*(currentPosition-halfWay));
        } else {
          //accelerate
          //v^2=u^2+2as
          v=sqrt(minSpeed*minSpeed+2*accel*(currentPosition-start));
        }
 /*   } else {
        if (currentPosition<halfWay) {
           //deaccelarate
           midPointVsqrd=minSpeed*minSpeed+2*accel*(start-halfWay);
           v=sqrt(midPointVsqrd-2*accel*(halfWay-currentPosition));
        } else {
          //accelerate
          //calc velocity
          //v^2=u^2+2as
          v=sqrt(minSpeed*minSpeed+2*accel*(start-currentPosition));
        } 
    } */

    if (v<minSpeed && v > 0) {
       v=minSpeed;
    }
    else if (abs(v) < minSpeed && v < 0){
        v = -1*minSpeed;
    } 

    if (v>topSpeed && v > 0) {
        v=topSpeed;
    }
    else if (abs(v) > topSpeed && v < 0){
        v = -1*topSpeed;
    }

    return v;
    //return topSpeed;
}