drive.cpp

00001 #include "driver.h"
00002 
00003 Driver::Driver(osPriority priority, int memory) : pidR(KP, KI, KD, 0.1, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, DIRECT),
00004 pidL(KP, KI, KD, PIDSAMPLERATE, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, REVERSE),
00005 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) {
00006     
00007     driver_thread(priority, memory);
00008     
00009     lSpeedGoal = 0;//Goal motor RPM
00010     rSpeedGoal = 0;
00011     l_RPM = 0;//Measured current speed
00012     r_RPM = 0;
00013     
00014     //my_led = DigitalOut(LED1);
00015     LoutA = 0x00; //Holds left output A of encoder signal
00016     LoutB = 0x00; //Holds left output B of encoder signal
00017     RoutA = 0x00; //Holds Right output A of encoder signal
00018     RoutB = 0x00; //Holds Right output B of encoder signal
00019     
00020     l_enc_val = 0x00; //Sequence that holds current and previous encoder combination values
00021     r_enc_val = 0x00;
00022     l_enc_direction = 1;
00023     r_enc_direction = 1;
00024     l_enc_count = 0; //Holds counter of encoder pulses
00025     r_enc_count = 0; //Holds counter of encoder pulses
00026     
00027     l_old_enc_count = 0x00; //Hold old encounter value for computation
00028     r_old_enc_count = 0x00;
00029 }
00030 
00031 void Driver::l_encode_trig() {
00032     static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
00033     LoutA = l_encoder2.read();
00034     LoutB = l_encoder1.read();
00035     
00036     l_enc_val = l_enc_val << 2;
00037     LoutA = LoutA <<1;
00038     l_enc_val = l_enc_val | LoutA | LoutB;
00039     l_enc_count += lookup_table[l_enc_val & 0x0F];    
00040 }
00041 
00042 void Driver::r_encode_trig() {
00043     static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
00044     RoutA = r_encoder2.read();
00045     RoutB = r_encoder1.read();
00046 
00047     r_enc_val = r_enc_val << 2;
00048     RoutA = RoutA <<1;
00049     r_enc_val = r_enc_val | RoutA | RoutB; 
00050     r_enc_count += lookup_table[r_enc_val & 0x0F];
00051 }
00052 
00053 void Driver::run() {
00054     float rSpeed, lSpeed;
00055     left_motor.period_us(PWMPERIOD);  
00056     left_motor_rev.period_us(PWMPERIOD);
00057     right_motor.period_us(PWMPERIOD); 
00058     right_motor_rev.period_us(PWMPERIOD);
00059 
00060     l_encoder1.rise(callback(this, &Driver::l_encode_trig));      //Out A of encoder rises
00061     l_encoder1.fall(callback(this, &Driver::l_encode_trig));      //Out A of encoder falls
00062     l_encoder2.rise(callback(this, &Driver::l_encode_trig));      //Out B of encoder rises
00063     l_encoder2.fall(callback(this, &Driver::l_encode_trig));      //Out B of encoder falls
00064     
00065     r_encoder1.rise(callback(this, &Driver::r_encode_trig));      //Out A of encoder rises
00066     r_encoder1.fall(callback(this, &Driver::r_encode_trig));      //Out A of encoder falls
00067     r_encoder2.rise(callback(this, &Driver::r_encode_trig));      //Out B of encoder rises
00068     r_encoder2.fall(callback(this, &Driver::r_encode_trig));      //Out B of encoder falls
00069     
00070     Timer timer;
00071     timer.start();
00072     int timerOld = 0;
00073     
00074     while(true) {
00075         Thread::wait(100);
00076         
00077         if(timer.read_ms() > 100) {
00078             timer.reset();
00079             
00080             /*right_motor.pulsewidth_us(2000);
00081             right_motor_rev.pulsewidth_us(0);
00082             left_motor.pulsewidth_us(2000);
00083             left_motor_rev.pulsewidth_us(0);*/
00084             /*Calculate rpm from encoder counts*/
00085             r_RPM = (r_enc_count - r_old_enc_count) * 5*60/(51.45*12);
00086             r_old_enc_count = r_enc_count;
00087             l_RPM = (l_enc_count - l_old_enc_count) * 5*60/(51.45*12);
00088             l_old_enc_count = l_enc_count;
00089             
00090             /*Give motor speeds to pid controller*/
00091             pidR.PIDInputSet(r_RPM);
00092             pidL.PIDInputSet(l_RPM);
00093             
00094             pidR.PIDCompute();
00095             pidL.PIDCompute();
00096             
00097             /*Get new pwm output from PID controller and set pwm on motors*/
00098             lock.lock();
00099             rSpeed = pidR.PIDOutputGet();
00100             lSpeed = pidL.PIDOutputGet();
00101             printf("r_RPM: [%d] rSpeed: [%f]\n\r", r_RPM, rSpeed);
00102             printf("l_RPM: [%d] lSpeed: [%f]\n\r", l_RPM, lSpeed);
00103             
00104             if (rSpeed > 0) {
00105                 right_motor_rev.pulsewidth_us(abs((long)rSpeed));
00106                 right_motor.pulsewidth_us(0);
00107             }
00108             else {
00109                 right_motor_rev.pulsewidth_us(0);
00110                 right_motor.pulsewidth_us(abs((long)rSpeed));
00111             }
00112             
00113             if (lSpeed > 0) {
00114                 left_motor_rev.pulsewidth_us(abs((long)lSpeed));
00115                 left_motor.pulsewidth_us(0);
00116             }
00117             else {
00118                 left_motor_rev.pulsewidth_us(0);
00119                 left_motor.pulsewidth_us(abs((long)lSpeed));
00120             }
00121             lock.unlock();
00122         }
00123         else {
00124             Thread::wait(1000);
00125         }
00126     }
00127 }
00128     
00129 void Driver::start() {
00130     driver_thread.start(callback(this, &Driver::run));
00131 }
00132 
00133 void Driver::setVelocity(int lSpeed, int rSpeed) {
00134     lock.lock();
00135     lSpeedGoal = lSpeed; //Goal motor speed NEEDS CONVERSION FROM ms-1 to RPM
00136     rSpeedGoal = rSpeed;
00137     pidR.PIDSetpointSet(rSpeedGoal);
00138     pidL.PIDSetpointSet(lSpeedGoal);
00139     lock.unlock();
00140 }