Revision 0:e5e9b52a35ae, committed 2017-08-23

Comitter:

williampeers

Date:

Wed Aug 23 02:25:59 2017 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r e5e9b52a35ae drive.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.cpp	Wed Aug 23 02:25:59 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

diff -r 000000000000 -r e5e9b52a35ae drive.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.h	Wed Aug 23 02:25:59 2017 +0000
@@ -0,0 +1,70 @@
+#ifndef __DRIVE_INCLUDED__
+#define __DRIVE_INCLUDED__
+
+#include "mbed.h"
+#include "pid_controller.h"
+
+#define PWMPERIOD 4096 //PWM period in us
+
+#define KP 3
+#define KI 0.1
+#define KD 0.1
+#define PIDSAMPLERATE 0.1
+
+class Driver {
+private:
+    PIDControl pidR;
+    PIDControl pidL;
+
+    InterruptIn l_encoder1; //Left motor encoder signal 1
+    InterruptIn l_encoder2;   //Left motor encoder signal 2
+    InterruptIn r_encoder1; //Right motor encoder signal 3
+    InterruptIn r_encoder2;   //Right motor encoder signal 4
+    
+    PwmOut left_motor; // Forward Motor direction
+    PwmOut left_motor_rev; // Reverse Motor direction
+    PwmOut right_motor; // Forward Motor direction
+    PwmOut right_motor_rev; // Reverse Motor direction
+
+    long lSpeedGoal;//Goal motor speed
+    long rSpeedGoal;
+    
+    long l_RPM;//Measured current speed
+    long r_RPM;
+    
+    Mutex lock;
+    
+    Thread driver_thread;
+    
+    unsigned int LoutA; //Holds left output A of encoder signal
+    unsigned int LoutB; //Holds left output B of encoder signal
+    unsigned int RoutA; //Holds Right output A of encoder signal
+    unsigned int RoutB; //Holds Right output B of encoder signal
+    
+    unsigned char l_enc_val; //Sequence that holds current and previous encoder combination values
+    unsigned char r_enc_val;
+    char l_enc_direction;
+    char r_enc_direction;
+    volatile long l_enc_count; //Holds current direction of left motor
+    volatile long r_enc_count; //Holds current direction of right motor
+    
+    //Ticker timer;
+    volatile long l_old_enc_count; //Hold old encounter value for computation
+    volatile long r_old_enc_count;
+
+    void l_encode_trig();//function for handling encoder trigger
+    void r_encode_trig();
+    
+    void run();//function to run in thread (contains infinite loop)
+    
+    
+    DigitalOut my_led;
+    
+public:
+    Driver(osPriority, int);
+    void start(); //starts thread "pid" running run()
+    //void setVelocity(int speed, char* directoin);//called to set desired speed
+    void setVelocity(int lSpeed, int rSpeed);
+};
+
+#endif
\ No newline at end of file