lab2
Dependencies: mbed hallsensor_software_decoder
main.cpp@1:47a1aaf98baa, 2019-03-29 (annotated)
- Committer:
- bobolee1239
- Date:
- Fri Mar 29 05:06:08 2019 +0000
- Revision:
- 1:47a1aaf98baa
- Parent:
- 0:b124e4e3a1e4
structure -- brian
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bobolee1239 | 1:47a1aaf98baa | 1 | // Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen |
bobolee1239 | 1:47a1aaf98baa | 2 | /*************************************************************** |
bobolee1239 | 1:47a1aaf98baa | 3 | ** MOBILE ROBOT LAB2 GROUP 4 [Vehicle Motion Control] |
bobolee1239 | 1:47a1aaf98baa | 4 | ** |
bobolee1239 | 1:47a1aaf98baa | 5 | ** - Members : Tsung-Han Lee, Tommy, Skyler |
bobolee1239 | 1:47a1aaf98baa | 6 | ** - NOTE : Modified from sample code provided |
bobolee1239 | 1:47a1aaf98baa | 7 | ** by NTHU Dynamic Systems and Control Lab |
bobolee1239 | 1:47a1aaf98baa | 8 | ** |
bobolee1239 | 1:47a1aaf98baa | 9 | ***************************************************************/ |
bobolee1239 | 0:b124e4e3a1e4 | 10 | #include "mbed.h" |
bobolee1239 | 0:b124e4e3a1e4 | 11 | #include "hallsensor_software_decoder.h" |
bobolee1239 | 0:b124e4e3a1e4 | 12 | |
bobolee1239 | 1:47a1aaf98baa | 13 | #define Ts 0.01 |
bobolee1239 | 0:b124e4e3a1e4 | 14 | |
bobolee1239 | 1:47a1aaf98baa | 15 | Ticker controllerTimer; |
bobolee1239 | 0:b124e4e3a1e4 | 16 | |
bobolee1239 | 1:47a1aaf98baa | 17 | /* declare pwm pin */ |
bobolee1239 | 0:b124e4e3a1e4 | 18 | PwmOut pwm1(D7); |
bobolee1239 | 0:b124e4e3a1e4 | 19 | PwmOut pwm1n(D11); |
bobolee1239 | 0:b124e4e3a1e4 | 20 | PwmOut pwm2(D8); |
bobolee1239 | 0:b124e4e3a1e4 | 21 | PwmOut pwm2n(A3); |
bobolee1239 | 0:b124e4e3a1e4 | 22 | |
bobolee1239 | 1:47a1aaf98baa | 23 | /********************* Helper Functions and Structures *******************/ |
bobolee1239 | 1:47a1aaf98baa | 24 | typedef volatile struct Controller { |
bobolee1239 | 1:47a1aaf98baa | 25 | volatile double rpm; // rotation speed in rpm |
bobolee1239 | 1:47a1aaf98baa | 26 | volatile double piOut; // PI Controller output |
bobolee1239 | 1:47a1aaf98baa | 27 | volatile double err; // error |
bobolee1239 | 1:47a1aaf98baa | 28 | volatile double ierr; // integration of error |
bobolee1239 | 1:47a1aaf98baa | 29 | double kp; // propotinal gain |
bobolee1239 | 1:47a1aaf98baa | 30 | double ki; // integrational gain |
bobolee1239 | 1:47a1aaf98baa | 31 | } Controller_t; |
bobolee1239 | 0:b124e4e3a1e4 | 32 | |
bobolee1239 | 1:47a1aaf98baa | 33 | /* Init Structure Instance */ |
bobolee1239 | 1:47a1aaf98baa | 34 | Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 1:47a1aaf98baa | 35 | Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 0:b124e4e3a1e4 | 36 | |
bobolee1239 | 1:47a1aaf98baa | 37 | volatile double refRPM1 = 25.0; // reference rpm1 |
bobolee1239 | 1:47a1aaf98baa | 38 | volatile double refRPM2 = -25.0; // reference rpm2 |
bobolee1239 | 0:b124e4e3a1e4 | 39 | |
bobolee1239 | 0:b124e4e3a1e4 | 40 | |
bobolee1239 | 1:47a1aaf98baa | 41 | void initTimer(); |
bobolee1239 | 1:47a1aaf98baa | 42 | void initPWM(); |
bobolee1239 | 1:47a1aaf98baa | 43 | void controllerTISR(); |
bobolee1239 | 1:47a1aaf98baa | 44 | /************************************************************************/ |
bobolee1239 | 0:b124e4e3a1e4 | 45 | |
bobolee1239 | 1:47a1aaf98baa | 46 | /* SERIAL DEBUG */ |
bobolee1239 | 0:b124e4e3a1e4 | 47 | //Serial pc(USBTX, USBRX); |
bobolee1239 | 0:b124e4e3a1e4 | 48 | |
bobolee1239 | 1:47a1aaf98baa | 49 | int main(int argc, char* argv[]) { |
bobolee1239 | 1:47a1aaf98baa | 50 | /* SERIAL DEBUG */ |
bobolee1239 | 1:47a1aaf98baa | 51 | // pc.baud(115200); |
bobolee1239 | 1:47a1aaf98baa | 52 | |
bobolee1239 | 1:47a1aaf98baa | 53 | initTimer(); |
bobolee1239 | 1:47a1aaf98baa | 54 | initPWM(); |
bobolee1239 | 0:b124e4e3a1e4 | 55 | init_CN(); |
bobolee1239 | 1:47a1aaf98baa | 56 | |
bobolee1239 | 1:47a1aaf98baa | 57 | while (1) { |
bobolee1239 | 1:47a1aaf98baa | 58 | // pc.printf("%.2f, %.2f \r\n", controller1.rpm, controller2.rpm); |
bobolee1239 | 1:47a1aaf98baa | 59 | wait_ms(100); |
bobolee1239 | 0:b124e4e3a1e4 | 60 | } |
bobolee1239 | 0:b124e4e3a1e4 | 61 | } |
bobolee1239 | 0:b124e4e3a1e4 | 62 | |
bobolee1239 | 1:47a1aaf98baa | 63 | void initTimer() { |
bobolee1239 | 1:47a1aaf98baa | 64 | controllerTimer.attach_us(&controllerTISR, 10000.0); |
bobolee1239 | 0:b124e4e3a1e4 | 65 | } |
bobolee1239 | 0:b124e4e3a1e4 | 66 | |
bobolee1239 | 1:47a1aaf98baa | 67 | void initPWM() { |
bobolee1239 | 0:b124e4e3a1e4 | 68 | pwm1.period_us(50); |
bobolee1239 | 0:b124e4e3a1e4 | 69 | pwm1.write(0.5); |
bobolee1239 | 0:b124e4e3a1e4 | 70 | TIM1->CCER |= 0x4; |
bobolee1239 | 1:47a1aaf98baa | 71 | |
bobolee1239 | 0:b124e4e3a1e4 | 72 | pwm2.period_us(50); |
bobolee1239 | 0:b124e4e3a1e4 | 73 | pwm2.write(0.5); |
bobolee1239 | 1:47a1aaf98baa | 74 | TIM1->CCER |= 0x40; |
bobolee1239 | 0:b124e4e3a1e4 | 75 | } |
bobolee1239 | 0:b124e4e3a1e4 | 76 | |
bobolee1239 | 1:47a1aaf98baa | 77 | void controllerTISR() { |
bobolee1239 | 1:47a1aaf98baa | 78 | /********* MOTOR 1 ******************/ |
bobolee1239 | 1:47a1aaf98baa | 79 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 1:47a1aaf98baa | 80 | controller1.rpm = (double)wheelState1.numStateChange * 2.232142857; |
bobolee1239 | 1:47a1aaf98baa | 81 | wheelState1.numStateChange = 0; |
bobolee1239 | 0:b124e4e3a1e4 | 82 | |
bobolee1239 | 1:47a1aaf98baa | 83 | controller1.err = refRPM1 - controller1.rpm; |
bobolee1239 | 1:47a1aaf98baa | 84 | controller1.ierr += Ts*controller1.err; |
bobolee1239 | 0:b124e4e3a1e4 | 85 | |
bobolee1239 | 1:47a1aaf98baa | 86 | if (controller1.ierr > 50.0) { |
bobolee1239 | 1:47a1aaf98baa | 87 | controller1.ierr = 50.0; |
bobolee1239 | 1:47a1aaf98baa | 88 | } else if (controller1.ierr < -50.0) { |
bobolee1239 | 1:47a1aaf98baa | 89 | controller1.ierr = -50.0; |
bobolee1239 | 1:47a1aaf98baa | 90 | } |
bobolee1239 | 1:47a1aaf98baa | 91 | controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr; |
bobolee1239 | 1:47a1aaf98baa | 92 | |
bobolee1239 | 1:47a1aaf98baa | 93 | if (controller1.piOut >= 0.5) { |
bobolee1239 | 1:47a1aaf98baa | 94 | controller1.piOut = 0.5; |
bobolee1239 | 1:47a1aaf98baa | 95 | } else if (controller1.piOut <= -0.5) { |
bobolee1239 | 1:47a1aaf98baa | 96 | controller1.piOut = -0.5; |
bobolee1239 | 0:b124e4e3a1e4 | 97 | } |
bobolee1239 | 0:b124e4e3a1e4 | 98 | |
bobolee1239 | 1:47a1aaf98baa | 99 | pwm1.write(controller1.piOut + 0.5); |
bobolee1239 | 0:b124e4e3a1e4 | 100 | TIM1->CCER |= 0x4; |
bobolee1239 | 0:b124e4e3a1e4 | 101 | |
bobolee1239 | 1:47a1aaf98baa | 102 | /********* MOTOR 2 ******************/ |
bobolee1239 | 1:47a1aaf98baa | 103 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 1:47a1aaf98baa | 104 | controller2.rpm = (double)wheelState2.numStateChange * 2.232142857; |
bobolee1239 | 1:47a1aaf98baa | 105 | wheelState2.numStateChange = 0; |
bobolee1239 | 0:b124e4e3a1e4 | 106 | |
bobolee1239 | 1:47a1aaf98baa | 107 | controller2.err = refRPM2 - controller2.rpm; |
bobolee1239 | 1:47a1aaf98baa | 108 | controller2.ierr += Ts*controller2.err; |
bobolee1239 | 0:b124e4e3a1e4 | 109 | |
bobolee1239 | 1:47a1aaf98baa | 110 | if (controller2.ierr > 50.0) { |
bobolee1239 | 1:47a1aaf98baa | 111 | controller2.ierr = 50.0; |
bobolee1239 | 1:47a1aaf98baa | 112 | } else if (controller2.ierr < -50.0) { |
bobolee1239 | 1:47a1aaf98baa | 113 | controller2.ierr = -50.0; |
bobolee1239 | 0:b124e4e3a1e4 | 114 | } |
bobolee1239 | 1:47a1aaf98baa | 115 | controller2.piOut = controller2.kp*controller2.err + controller2.ki*controller2.ierr; |
bobolee1239 | 1:47a1aaf98baa | 116 | |
bobolee1239 | 1:47a1aaf98baa | 117 | if (controller2.piOut >= 0.5) { |
bobolee1239 | 1:47a1aaf98baa | 118 | controller2.piOut = 0.5; |
bobolee1239 | 1:47a1aaf98baa | 119 | } else if (controller2.piOut <= -0.5) { |
bobolee1239 | 1:47a1aaf98baa | 120 | controller2.piOut = -0.5; |
bobolee1239 | 0:b124e4e3a1e4 | 121 | } |
bobolee1239 | 1:47a1aaf98baa | 122 | |
bobolee1239 | 1:47a1aaf98baa | 123 | pwm2.write(-controller2.piOut + 0.5); |
bobolee1239 | 0:b124e4e3a1e4 | 124 | TIM1->CCER |= 0x40; |
bobolee1239 | 1:47a1aaf98baa | 125 | } |