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