Diff: Drive/drive.cpp

Revision:

0:91703b1eb29e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drive/drive.cpp	Wed Aug 23 02:10:36 2017 +0000
@@ -0,0 +1,140 @@
+#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();
+}
\ No newline at end of file