mobile_robot_tea / Mbed 2 deprecated motor_control

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?

UserRevisionLine numberNew 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 }