Drive thread library
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 22:09:10 by 1.7.2