File content as of revision 1:87d8ac1a1e94:

#include "driver.h"

Driver::Driver(osPriority priority, int memory) : pidR(KP, KI, KD, 0.1, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, DIRECT),
pidL(KP, KI, KD, PIDSAMPLERATE, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, REVERSE),
l_encoder1(PB_4), l_encoder2(PB_5), r_encoder1(PA_0), r_encoder2(PA_1), left_motor(PB_10), left_motor_rev(PA_8), right_motor(PA_15), right_motor_rev(PB_7), my_led(LED1) {
    
    driver_thread(priority, memory);
    
    lSpeedGoal = 0;//Goal motor RPM
    rSpeedGoal = 0;
    l_RPM = 0;//Measured current speed
    r_RPM = 0;
    
    //my_led = DigitalOut(LED1);
    LoutA = 0x00; //Holds left output A of encoder signal
    LoutB = 0x00; //Holds left output B of encoder signal
    RoutA = 0x00; //Holds Right output A of encoder signal
    RoutB = 0x00; //Holds Right output B of encoder signal
    
    l_enc_val = 0x00; //Sequence that holds current and previous encoder combination values
    r_enc_val = 0x00;
    l_enc_direction = 1;
    r_enc_direction = 1;
    l_enc_count = 0; //Holds counter of encoder pulses
    r_enc_count = 0; //Holds counter of encoder pulses
    
    l_old_enc_count = 0x00; //Hold old encounter value for computation
    r_old_enc_count = 0x00;
}

void Driver::l_encode_trig() {
    static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
    LoutA = l_encoder2.read();
    LoutB = l_encoder1.read();
    
    l_enc_val = l_enc_val << 2;
    LoutA = LoutA <<1;
    l_enc_val = l_enc_val | LoutA | LoutB;
    l_enc_count += lookup_table[l_enc_val & 0x0F];    
}

void Driver::r_encode_trig() {
    static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
    RoutA = r_encoder2.read();
    RoutB = r_encoder1.read();

    r_enc_val = r_enc_val << 2;
    RoutA = RoutA <<1;
    r_enc_val = r_enc_val | RoutA | RoutB; 
    r_enc_count += lookup_table[r_enc_val & 0x0F];
}

void Driver::run() {
    float rSpeed, lSpeed;
    left_motor.period_us(PWMPERIOD);  
    left_motor_rev.period_us(PWMPERIOD);
    right_motor.period_us(PWMPERIOD); 
    right_motor_rev.period_us(PWMPERIOD);

    l_encoder1.rise(callback(this, &Driver::l_encode_trig));      //Out A of encoder rises
    l_encoder1.fall(callback(this, &Driver::l_encode_trig));      //Out A of encoder falls
    l_encoder2.rise(callback(this, &Driver::l_encode_trig));      //Out B of encoder rises
    l_encoder2.fall(callback(this, &Driver::l_encode_trig));      //Out B of encoder falls
    
    r_encoder1.rise(callback(this, &Driver::r_encode_trig));      //Out A of encoder rises
    r_encoder1.fall(callback(this, &Driver::r_encode_trig));      //Out A of encoder falls
    r_encoder2.rise(callback(this, &Driver::r_encode_trig));      //Out B of encoder rises
    r_encoder2.fall(callback(this, &Driver::r_encode_trig));      //Out B of encoder falls
    
    Timer timer;
    timer.start();
    int timerOld = 0;
    
    while(true) {
        Thread::wait(100);
        
        if(timer.read_ms() > 100) {
            timer.reset();
            
            /*right_motor.pulsewidth_us(2000);
            right_motor_rev.pulsewidth_us(0);
            left_motor.pulsewidth_us(2000);
            left_motor_rev.pulsewidth_us(0);*/
            /*Calculate rpm from encoder counts*/
            r_RPM = (r_enc_count - r_old_enc_count) * 5*60/(51.45*12);
            r_old_enc_count = r_enc_count;
            l_RPM = (l_enc_count - l_old_enc_count) * 5*60/(51.45*12);
            l_old_enc_count = l_enc_count;
            
            /*Give motor speeds to pid controller*/
            pidR.PIDInputSet(r_RPM);
            pidL.PIDInputSet(l_RPM);
            
            pidR.PIDCompute();
            pidL.PIDCompute();
            
            /*Get new pwm output from PID controller and set pwm on motors*/
            lock.lock();
            rSpeed = pidR.PIDOutputGet();
            lSpeed = pidL.PIDOutputGet();
            printf("r_RPM: [%d] rSpeed: [%f]\n\r", r_RPM, rSpeed);
            printf("l_RPM: [%d] lSpeed: [%f]\n\r", l_RPM, lSpeed);
            
            if (rSpeed > 0) {
                right_motor_rev.pulsewidth_us(abs((long)rSpeed));
                right_motor.pulsewidth_us(0);
            }
            else {
                right_motor_rev.pulsewidth_us(0);
                right_motor.pulsewidth_us(abs((long)rSpeed));
            }
            
            if (lSpeed > 0) {
                left_motor_rev.pulsewidth_us(abs((long)lSpeed));
                left_motor.pulsewidth_us(0);
            }
            else {
                left_motor_rev.pulsewidth_us(0);
                left_motor.pulsewidth_us(abs((long)lSpeed));
            }
            lock.unlock();
        }
        else {
            Thread::wait(1000);
        }
    }
}
    
void Driver::start() {
    driver_thread.start(callback(this, &Driver::run));
}

void Driver::setVelocity(int lSpeed, int rSpeed) {
    lock.lock();
    lSpeedGoal = lSpeed; //Goal motor speed NEEDS CONVERSION FROM ms-1 to RPM
    rSpeedGoal = rSpeed;
    pidR.PIDSetpointSet(rSpeedGoal);
    pidL.PIDSetpointSet(lSpeedGoal);
    lock.unlock();
}