lab3
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
main.cpp@3:a308ddde9078, 2019-03-27 (annotated)
- Committer:
- bobolee1239
- Date:
- Wed Mar 27 03:02:46 2019 +0000
- Revision:
- 3:a308ddde9078
- Parent:
- 2:ff7641d822df
- Child:
- 5:eec419520d71
add comment
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bobolee1239 | 3:a308ddde9078 | 1 | // Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen |
bobolee1239 | 2:ff7641d822df | 2 | /*************************************************************** |
bobolee1239 | 2:ff7641d822df | 3 | ** MOBILE ROBOT LAB3 GROUP 4 [Vehicle Motion Control] |
bobolee1239 | 2:ff7641d822df | 4 | ** |
bobolee1239 | 2:ff7641d822df | 5 | ** - Members : Tsung-Han Lee, Tommy, Skyler |
bobolee1239 | 2:ff7641d822df | 6 | ** - NOTE : Modified from sample code provided |
bobolee1239 | 2:ff7641d822df | 7 | ** by NTHU Dynamic Systems and Control Lab |
bobolee1239 | 2:ff7641d822df | 8 | ** |
bobolee1239 | 2:ff7641d822df | 9 | ***************************************************************/ |
bobolee1239 | 2:ff7641d822df | 10 | #include <ros.h> |
bobolee1239 | 2:ff7641d822df | 11 | #include <geometry_msgs/Twist.h> |
bobolee1239 | 0:1d023e270dfa | 12 | #include "mbed.h" |
bobolee1239 | 2:ff7641d822df | 13 | #include "hallsensor_software_decoder.h" |
bobolee1239 | 2:ff7641d822df | 14 | |
bobolee1239 | 2:ff7641d822df | 15 | #define Ts 0.01 |
bobolee1239 | 2:ff7641d822df | 16 | |
bobolee1239 | 2:ff7641d822df | 17 | Ticker controllerTimer; |
bobolee1239 | 2:ff7641d822df | 18 | |
bobolee1239 | 2:ff7641d822df | 19 | /* declare pwm pin */ |
bobolee1239 | 2:ff7641d822df | 20 | PwmOut pwm1(D7); |
bobolee1239 | 2:ff7641d822df | 21 | PwmOut pwm1n(D11); |
bobolee1239 | 2:ff7641d822df | 22 | PwmOut pwm2(D8); |
bobolee1239 | 2:ff7641d822df | 23 | PwmOut pwm2n(A3); |
bobolee1239 | 2:ff7641d822df | 24 | |
bobolee1239 | 2:ff7641d822df | 25 | /****************** Helper Functions and Structures ****************/ |
bobolee1239 | 2:ff7641d822df | 26 | void initTimer(); |
bobolee1239 | 2:ff7641d822df | 27 | void initPWM(); |
bobolee1239 | 2:ff7641d822df | 28 | void controllerTISR(); |
bobolee1239 | 2:ff7641d822df | 29 | void messageCallback(const geometry_msgs::Twist& recvMsg); |
bobolee1239 | 0:1d023e270dfa | 30 | |
bobolee1239 | 2:ff7641d822df | 31 | typedef volatile struct Controller { |
bobolee1239 | 2:ff7641d822df | 32 | volatile double rpm; // rotation speed in rpm |
bobolee1239 | 2:ff7641d822df | 33 | volatile double refRPM; // reference rpm |
bobolee1239 | 2:ff7641d822df | 34 | volatile double pwmDuty; // duty cycle of pwm |
bobolee1239 | 2:ff7641d822df | 35 | volatile double piOut; // PI Controller output |
bobolee1239 | 2:ff7641d822df | 36 | volatile double err; // error |
bobolee1239 | 2:ff7641d822df | 37 | volatile double ierr; // integration of error |
bobolee1239 | 2:ff7641d822df | 38 | double kp; // propotinal gain |
bobolee1239 | 2:ff7641d822df | 39 | double ki; // integrational gain |
bobolee1239 | 2:ff7641d822df | 40 | } Controller_t; |
bobolee1239 | 2:ff7641d822df | 41 | |
bobolee1239 | 2:ff7641d822df | 42 | typedef volatile struct Vehicle { |
bobolee1239 | 2:ff7641d822df | 43 | double r; // radius |
bobolee1239 | 2:ff7641d822df | 44 | double L; // distance between two wheel |
bobolee1239 | 2:ff7641d822df | 45 | volatile double refV; // reference forward speed |
bobolee1239 | 2:ff7641d822df | 46 | volatile double refW; // reference forward speed |
bobolee1239 | 2:ff7641d822df | 47 | volatile double V; // forward speed |
bobolee1239 | 2:ff7641d822df | 48 | volatile double W; // rotation speed |
bobolee1239 | 2:ff7641d822df | 49 | } Vehicle_t; |
bobolee1239 | 2:ff7641d822df | 50 | /******************************************************************/ |
bobolee1239 | 2:ff7641d822df | 51 | |
bobolee1239 | 2:ff7641d822df | 52 | /* Init Structure Instance */ |
bobolee1239 | 2:ff7641d822df | 53 | Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 2:ff7641d822df | 54 | Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 2:ff7641d822df | 55 | Vehicle_t car = {0.03, 0.27, 0.0, 0.0, 0.0, 0.0}; |
bobolee1239 | 2:ff7641d822df | 56 | |
bobolee1239 | 2:ff7641d822df | 57 | int main(int argc, char* argv[]) { |
bobolee1239 | 2:ff7641d822df | 58 | initTimer(); |
bobolee1239 | 2:ff7641d822df | 59 | initPWM(); |
bobolee1239 | 2:ff7641d822df | 60 | init_CN(); |
bobolee1239 | 0:1d023e270dfa | 61 | |
bobolee1239 | 2:ff7641d822df | 62 | /************************ ROS Parameters ***************************/ |
bobolee1239 | 2:ff7641d822df | 63 | ros::NodeHandle nh; |
bobolee1239 | 2:ff7641d822df | 64 | geometry_msgs::Twist vel_msg; |
bobolee1239 | 2:ff7641d822df | 65 | |
bobolee1239 | 2:ff7641d822df | 66 | ros::Publisher pub("feedback_Vel", &vel_msg); |
bobolee1239 | 2:ff7641d822df | 67 | ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCallback); |
bobolee1239 | 2:ff7641d822df | 68 | /*******************************************************************/ |
bobolee1239 | 2:ff7641d822df | 69 | |
bobolee1239 | 2:ff7641d822df | 70 | nh.initNode(); |
bobolee1239 | 2:ff7641d822df | 71 | nh.subscribe(sub); |
bobolee1239 | 2:ff7641d822df | 72 | nh.advertise(pub); |
bobolee1239 | 2:ff7641d822df | 73 | |
bobolee1239 | 2:ff7641d822df | 74 | while (1) { |
bobolee1239 | 2:ff7641d822df | 75 | /************************* |
bobolee1239 | 2:ff7641d822df | 76 | ** 1. 2pi/60 = 0.1047197551 |
bobolee1239 | 2:ff7641d822df | 77 | ** 2. 2pi/60/2 = 0.05235987756 |
bobolee1239 | 2:ff7641d822df | 78 | **************************/ |
bobolee1239 | 2:ff7641d822df | 79 | car.V = (controller1.rpm + controller2.rpm) * car.r * 0.05235987756; |
bobolee1239 | 2:ff7641d822df | 80 | car.W = -1.0 * car.r * (controller1.rpm - controller2.rpm) / car.L * 0.1047197551; |
bobolee1239 | 2:ff7641d822df | 81 | |
bobolee1239 | 2:ff7641d822df | 82 | vel_msg.linear.x = car.V; |
bobolee1239 | 2:ff7641d822df | 83 | vel_msg.angular.z = car.W; |
bobolee1239 | 2:ff7641d822df | 84 | |
bobolee1239 | 2:ff7641d822df | 85 | pub.publish(&vel_msg); |
bobolee1239 | 2:ff7641d822df | 86 | |
bobolee1239 | 2:ff7641d822df | 87 | nh.spinOnce(); |
bobolee1239 | 2:ff7641d822df | 88 | |
bobolee1239 | 2:ff7641d822df | 89 | wait_ms(100); |
bobolee1239 | 0:1d023e270dfa | 90 | } |
bobolee1239 | 0:1d023e270dfa | 91 | } |
bobolee1239 | 2:ff7641d822df | 92 | |
bobolee1239 | 2:ff7641d822df | 93 | void initTimer() { |
bobolee1239 | 2:ff7641d822df | 94 | controllerTimer.attach_us(&controllerTISR, 10000.0); |
bobolee1239 | 2:ff7641d822df | 95 | } |
bobolee1239 | 2:ff7641d822df | 96 | |
bobolee1239 | 2:ff7641d822df | 97 | void initPWM() { |
bobolee1239 | 2:ff7641d822df | 98 | pwm1.period_us(50); |
bobolee1239 | 2:ff7641d822df | 99 | pwm1.write(0.5); |
bobolee1239 | 2:ff7641d822df | 100 | TIM1->CCER |= 0x4; |
bobolee1239 | 2:ff7641d822df | 101 | |
bobolee1239 | 2:ff7641d822df | 102 | pwm2.period_us(50); |
bobolee1239 | 2:ff7641d822df | 103 | pwm2.write(0.5); |
bobolee1239 | 2:ff7641d822df | 104 | TIM1->CCER |= 0x40; |
bobolee1239 | 2:ff7641d822df | 105 | } |
bobolee1239 | 2:ff7641d822df | 106 | |
bobolee1239 | 2:ff7641d822df | 107 | void controllerTISR() { |
bobolee1239 | 2:ff7641d822df | 108 | // 60.0 / 2pi = 9.549296586 |
bobolee1239 | 2:ff7641d822df | 109 | controller1.refRPM = (car.refV/car.r - car.L*car.refW/2.0/car.r) / 9.549296586; |
bobolee1239 | 2:ff7641d822df | 110 | controller2.refRPM = (car.refV/car.r + car.L*car.refW/2.0/car.r) / 9.549296586; |
bobolee1239 | 2:ff7641d822df | 111 | |
bobolee1239 | 2:ff7641d822df | 112 | /********* MOTOR 1 ******************/ |
bobolee1239 | 2:ff7641d822df | 113 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 2:ff7641d822df | 114 | controller1.rpm = (double)wheelState1.numStateChange * 2.232142857; |
bobolee1239 | 2:ff7641d822df | 115 | wheelState1.numStateChange = 0; |
bobolee1239 | 2:ff7641d822df | 116 | |
bobolee1239 | 2:ff7641d822df | 117 | controller1.err = controller1.refRPM - controller1.rpm; |
bobolee1239 | 2:ff7641d822df | 118 | controller1.ierr += Ts*controller1.err; |
bobolee1239 | 2:ff7641d822df | 119 | |
bobolee1239 | 2:ff7641d822df | 120 | if (controller1.ierr > 50.0) { |
bobolee1239 | 2:ff7641d822df | 121 | controller1.ierr = 50.0; |
bobolee1239 | 2:ff7641d822df | 122 | } else if (controller1.ierr < -50.0) { |
bobolee1239 | 2:ff7641d822df | 123 | controller1.ierr = -50.0; |
bobolee1239 | 2:ff7641d822df | 124 | } |
bobolee1239 | 2:ff7641d822df | 125 | controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr; |
bobolee1239 | 2:ff7641d822df | 126 | |
bobolee1239 | 2:ff7641d822df | 127 | if (controller1.piOut > 0.5) { |
bobolee1239 | 2:ff7641d822df | 128 | controller1.piOut = 0.5; |
bobolee1239 | 2:ff7641d822df | 129 | } else if (controller1.piOut < -0.5) { |
bobolee1239 | 2:ff7641d822df | 130 | controller1.piOut = -0.5; |
bobolee1239 | 2:ff7641d822df | 131 | } |
bobolee1239 | 2:ff7641d822df | 132 | |
bobolee1239 | 2:ff7641d822df | 133 | pwm1.write(controller1.piOut + 0.5); |
bobolee1239 | 2:ff7641d822df | 134 | TIM1->CCER |= 0x4; |
bobolee1239 | 2:ff7641d822df | 135 | |
bobolee1239 | 2:ff7641d822df | 136 | /********* MOTOR 2 ******************/ |
bobolee1239 | 2:ff7641d822df | 137 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 2:ff7641d822df | 138 | controller2.rpm = (double)wheelState2.numStateChange * 2.232142857; |
bobolee1239 | 2:ff7641d822df | 139 | wheelState2.numStateChange = 0; |
bobolee1239 | 2:ff7641d822df | 140 | |
bobolee1239 | 2:ff7641d822df | 141 | controller2.err = controller2.refRPM - controller2.rpm; |
bobolee1239 | 2:ff7641d822df | 142 | controller2.ierr += Ts*controller2.err; |
bobolee1239 | 2:ff7641d822df | 143 | |
bobolee1239 | 2:ff7641d822df | 144 | if (controller2.ierr > 50.0) { |
bobolee1239 | 2:ff7641d822df | 145 | controller2.ierr = 50.0; |
bobolee1239 | 2:ff7641d822df | 146 | } else if (controller2.ierr < -50.0) { |
bobolee1239 | 2:ff7641d822df | 147 | controller2.ierr = -50.0; |
bobolee1239 | 2:ff7641d822df | 148 | } |
bobolee1239 | 2:ff7641d822df | 149 | controller2.piOut = controller2.kp*controller2.err + controller2.ki*controller2.ierr; |
bobolee1239 | 2:ff7641d822df | 150 | |
bobolee1239 | 2:ff7641d822df | 151 | if (controller2.piOut > 0.5) { |
bobolee1239 | 2:ff7641d822df | 152 | controller2.piOut = 0.5; |
bobolee1239 | 2:ff7641d822df | 153 | } else if (controller2.piOut < -0.5) { |
bobolee1239 | 2:ff7641d822df | 154 | controller2.piOut = -0.5; |
bobolee1239 | 2:ff7641d822df | 155 | } |
bobolee1239 | 2:ff7641d822df | 156 | |
bobolee1239 | 2:ff7641d822df | 157 | pwm1.write(controller2.piOut + 0.5); |
bobolee1239 | 2:ff7641d822df | 158 | TIM1->CCER |= 0x40; |
bobolee1239 | 2:ff7641d822df | 159 | } |
bobolee1239 | 2:ff7641d822df | 160 | |
bobolee1239 | 2:ff7641d822df | 161 | void messageCallback(const geometry_msgs::Twist& recvMsg) { |
bobolee1239 | 2:ff7641d822df | 162 | car.refV = 0.25 * recvMsg.linear.x; |
bobolee1239 | 2:ff7641d822df | 163 | car.refW = recvMsg.angular.z; |
bobolee1239 | 2:ff7641d822df | 164 | } |