Denver Hayward / Mbed OS Robot

Massey 2017, Group 5, AGV control software.

Dependencies:   Commands charQueue esp8266-driver

Drive/drive.cpp@1:87d8ac1a1e94, 2017-08-23 (annotated)

Committer:
williampeers
Date:
Wed Aug 23 02:17:18 2017 +0000
Revision:
1:87d8ac1a1e94
Parent:
0:91703b1eb29e 
First edition. Mostly just templates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
williampeers 0:91703b1eb29e 1 #include "driver.h"
williampeers 0:91703b1eb29e 2
williampeers 0:91703b1eb29e 3 Driver::Driver(osPriority priority, int memory) : pidR(KP, KI, KD, 0.1, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, DIRECT),
williampeers 0:91703b1eb29e 4 pidL(KP, KI, KD, PIDSAMPLERATE, -1*PWMPERIOD, PWMPERIOD, AUTOMATIC, REVERSE),
williampeers 0:91703b1eb29e 5 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) {
williampeers 0:91703b1eb29e 6
williampeers 0:91703b1eb29e 7 driver_thread(priority, memory);
williampeers 0:91703b1eb29e 8
williampeers 0:91703b1eb29e 9 lSpeedGoal = 0;//Goal motor RPM
williampeers 0:91703b1eb29e 10 rSpeedGoal = 0;
williampeers 0:91703b1eb29e 11 l_RPM = 0;//Measured current speed
williampeers 0:91703b1eb29e 12 r_RPM = 0;
williampeers 0:91703b1eb29e 13
williampeers 0:91703b1eb29e 14 //my_led = DigitalOut(LED1);
williampeers 0:91703b1eb29e 15 LoutA = 0x00; //Holds left output A of encoder signal
williampeers 0:91703b1eb29e 16 LoutB = 0x00; //Holds left output B of encoder signal
williampeers 0:91703b1eb29e 17 RoutA = 0x00; //Holds Right output A of encoder signal
williampeers 0:91703b1eb29e 18 RoutB = 0x00; //Holds Right output B of encoder signal
williampeers 0:91703b1eb29e 19
williampeers 0:91703b1eb29e 20 l_enc_val = 0x00; //Sequence that holds current and previous encoder combination values
williampeers 0:91703b1eb29e 21 r_enc_val = 0x00;
williampeers 0:91703b1eb29e 22 l_enc_direction = 1;
williampeers 0:91703b1eb29e 23 r_enc_direction = 1;
williampeers 0:91703b1eb29e 24 l_enc_count = 0; //Holds counter of encoder pulses
williampeers 0:91703b1eb29e 25 r_enc_count = 0; //Holds counter of encoder pulses
williampeers 0:91703b1eb29e 26
williampeers 0:91703b1eb29e 27 l_old_enc_count = 0x00; //Hold old encounter value for computation
williampeers 0:91703b1eb29e 28 r_old_enc_count = 0x00;
williampeers 0:91703b1eb29e 29 }
williampeers 0:91703b1eb29e 30
williampeers 0:91703b1eb29e 31 void Driver::l_encode_trig() {
williampeers 0:91703b1eb29e 32 static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
williampeers 0:91703b1eb29e 33 LoutA = l_encoder2.read();
williampeers 0:91703b1eb29e 34 LoutB = l_encoder1.read();
williampeers 0:91703b1eb29e 35
williampeers 0:91703b1eb29e 36 l_enc_val = l_enc_val << 2;
williampeers 0:91703b1eb29e 37 LoutA = LoutA <<1;
williampeers 0:91703b1eb29e 38 l_enc_val = l_enc_val | LoutA | LoutB;
williampeers 0:91703b1eb29e 39 l_enc_count += lookup_table[l_enc_val & 0x0F];
williampeers 0:91703b1eb29e 40 }
williampeers 0:91703b1eb29e 41
williampeers 0:91703b1eb29e 42 void Driver::r_encode_trig() {
williampeers 0:91703b1eb29e 43 static int8_t lookup_table[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
williampeers 0:91703b1eb29e 44 RoutA = r_encoder2.read();
williampeers 0:91703b1eb29e 45 RoutB = r_encoder1.read();
williampeers 0:91703b1eb29e 46
williampeers 0:91703b1eb29e 47 r_enc_val = r_enc_val << 2;
williampeers 0:91703b1eb29e 48 RoutA = RoutA <<1;
williampeers 0:91703b1eb29e 49 r_enc_val = r_enc_val | RoutA | RoutB;
williampeers 0:91703b1eb29e 50 r_enc_count += lookup_table[r_enc_val & 0x0F];
williampeers 0:91703b1eb29e 51 }
williampeers 0:91703b1eb29e 52
williampeers 0:91703b1eb29e 53 void Driver::run() {
williampeers 0:91703b1eb29e 54 float rSpeed, lSpeed;
williampeers 0:91703b1eb29e 55 left_motor.period_us(PWMPERIOD);
williampeers 0:91703b1eb29e 56 left_motor_rev.period_us(PWMPERIOD);
williampeers 0:91703b1eb29e 57 right_motor.period_us(PWMPERIOD);
williampeers 0:91703b1eb29e 58 right_motor_rev.period_us(PWMPERIOD);
williampeers 0:91703b1eb29e 59
williampeers 0:91703b1eb29e 60 l_encoder1.rise(callback(this, &Driver::l_encode_trig)); //Out A of encoder rises
williampeers 0:91703b1eb29e 61 l_encoder1.fall(callback(this, &Driver::l_encode_trig)); //Out A of encoder falls
williampeers 0:91703b1eb29e 62 l_encoder2.rise(callback(this, &Driver::l_encode_trig)); //Out B of encoder rises
williampeers 0:91703b1eb29e 63 l_encoder2.fall(callback(this, &Driver::l_encode_trig)); //Out B of encoder falls
williampeers 0:91703b1eb29e 64
williampeers 0:91703b1eb29e 65 r_encoder1.rise(callback(this, &Driver::r_encode_trig)); //Out A of encoder rises
williampeers 0:91703b1eb29e 66 r_encoder1.fall(callback(this, &Driver::r_encode_trig)); //Out A of encoder falls
williampeers 0:91703b1eb29e 67 r_encoder2.rise(callback(this, &Driver::r_encode_trig)); //Out B of encoder rises
williampeers 0:91703b1eb29e 68 r_encoder2.fall(callback(this, &Driver::r_encode_trig)); //Out B of encoder falls
williampeers 0:91703b1eb29e 69
williampeers 0:91703b1eb29e 70 Timer timer;
williampeers 0:91703b1eb29e 71 timer.start();
williampeers 0:91703b1eb29e 72 int timerOld = 0;
williampeers 0:91703b1eb29e 73
williampeers 0:91703b1eb29e 74 while(true) {
williampeers 0:91703b1eb29e 75 Thread::wait(100);
williampeers 0:91703b1eb29e 76
williampeers 0:91703b1eb29e 77 if(timer.read_ms() > 100) {
williampeers 0:91703b1eb29e 78 timer.reset();
williampeers 0:91703b1eb29e 79
williampeers 0:91703b1eb29e 80 /*right_motor.pulsewidth_us(2000);
williampeers 0:91703b1eb29e 81 right_motor_rev.pulsewidth_us(0);
williampeers 0:91703b1eb29e 82 left_motor.pulsewidth_us(2000);
williampeers 0:91703b1eb29e 83 left_motor_rev.pulsewidth_us(0);*/
williampeers 0:91703b1eb29e 84 /*Calculate rpm from encoder counts*/
williampeers 0:91703b1eb29e 85 r_RPM = (r_enc_count - r_old_enc_count) * 5*60/(51.45*12);
williampeers 0:91703b1eb29e 86 r_old_enc_count = r_enc_count;
williampeers 0:91703b1eb29e 87 l_RPM = (l_enc_count - l_old_enc_count) * 5*60/(51.45*12);
williampeers 0:91703b1eb29e 88 l_old_enc_count = l_enc_count;
williampeers 0:91703b1eb29e 89
williampeers 0:91703b1eb29e 90 /*Give motor speeds to pid controller*/
williampeers 0:91703b1eb29e 91 pidR.PIDInputSet(r_RPM);
williampeers 0:91703b1eb29e 92 pidL.PIDInputSet(l_RPM);
williampeers 0:91703b1eb29e 93
williampeers 0:91703b1eb29e 94 pidR.PIDCompute();
williampeers 0:91703b1eb29e 95 pidL.PIDCompute();
williampeers 0:91703b1eb29e 96
williampeers 0:91703b1eb29e 97 /*Get new pwm output from PID controller and set pwm on motors*/
williampeers 0:91703b1eb29e 98 lock.lock();
williampeers 0:91703b1eb29e 99 rSpeed = pidR.PIDOutputGet();
williampeers 0:91703b1eb29e 100 lSpeed = pidL.PIDOutputGet();
williampeers 0:91703b1eb29e 101 printf("r_RPM: [%d] rSpeed: [%f]\n\r", r_RPM, rSpeed);
williampeers 0:91703b1eb29e 102 printf("l_RPM: [%d] lSpeed: [%f]\n\r", l_RPM, lSpeed);
williampeers 0:91703b1eb29e 103
williampeers 0:91703b1eb29e 104 if (rSpeed > 0) {
williampeers 0:91703b1eb29e 105 right_motor_rev.pulsewidth_us(abs((long)rSpeed));
williampeers 0:91703b1eb29e 106 right_motor.pulsewidth_us(0);
williampeers 0:91703b1eb29e 107 }
williampeers 0:91703b1eb29e 108 else {
williampeers 0:91703b1eb29e 109 right_motor_rev.pulsewidth_us(0);
williampeers 0:91703b1eb29e 110 right_motor.pulsewidth_us(abs((long)rSpeed));
williampeers 0:91703b1eb29e 111 }
williampeers 0:91703b1eb29e 112
williampeers 0:91703b1eb29e 113 if (lSpeed > 0) {
williampeers 0:91703b1eb29e 114 left_motor_rev.pulsewidth_us(abs((long)lSpeed));
williampeers 0:91703b1eb29e 115 left_motor.pulsewidth_us(0);
williampeers 0:91703b1eb29e 116 }
williampeers 0:91703b1eb29e 117 else {
williampeers 0:91703b1eb29e 118 left_motor_rev.pulsewidth_us(0);
williampeers 0:91703b1eb29e 119 left_motor.pulsewidth_us(abs((long)lSpeed));
williampeers 0:91703b1eb29e 120 }
williampeers 0:91703b1eb29e 121 lock.unlock();
williampeers 0:91703b1eb29e 122 }
williampeers 0:91703b1eb29e 123 else {
williampeers 0:91703b1eb29e 124 Thread::wait(1000);
williampeers 0:91703b1eb29e 125 }
williampeers 0:91703b1eb29e 126 }
williampeers 0:91703b1eb29e 127 }
williampeers 0:91703b1eb29e 128
williampeers 0:91703b1eb29e 129 void Driver::start() {
williampeers 0:91703b1eb29e 130 driver_thread.start(callback(this, &Driver::run));
williampeers 0:91703b1eb29e 131 }
williampeers 0:91703b1eb29e 132
williampeers 0:91703b1eb29e 133 void Driver::setVelocity(int lSpeed, int rSpeed) {
williampeers 0:91703b1eb29e 134 lock.lock();
williampeers 0:91703b1eb29e 135 lSpeedGoal = lSpeed; //Goal motor speed NEEDS CONVERSION FROM ms-1 to RPM
williampeers 0:91703b1eb29e 136 rSpeedGoal = rSpeed;
williampeers 0:91703b1eb29e 137 pidR.PIDSetpointSet(rSpeedGoal);
williampeers 0:91703b1eb29e 138 pidL.PIDSetpointSet(lSpeedGoal);
williampeers 0:91703b1eb29e 139 lock.unlock();
williampeers 0:91703b1eb29e 140 }