File content as of revision 10:63381713610c:

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


int main()
{
    float pos_change = 0; //temp variable for speed control
    float speed_error = 0; //temp variable for speed control

    float Kp = 0.02; //proportional constant
    float Ki = 0.05; //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 - float(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_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*0.1*speed_error + Ki*0.1*integral[i]; //speed proportional control
                }
            }

            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);
            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)
{
//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
    bool direction;

    if(end<0)
        direction = false;
    else
        direction = true;

    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=abs(start)+(abs(end)-abs(start))/2;


    if (abs(currentPosition)>halfWay) {
        //deaccelarate
        midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-abs(start));
        v=sqrt(midPointVsqrd-2*accel*(abs(currentPosition)-halfWay));
    } else {
        //accelerate
        //v^2=u^2+2as
        v=sqrt(minSpeed*minSpeed+2*accel*(abs(currentPosition)-abs(start)));
    }


    if (v<minSpeed) {
        v=minSpeed;
    } else if (v>topSpeed) {
        v = topSpeed;
    }

    if (direction == false) {
        v = -1*v;
    }

    return v;
}