lab3
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
main.cpp@6:2c7b0e609713, 2019-03-29 (annotated)
- Committer:
- bobolee1239
- Date:
- Fri Mar 29 05:05:23 2019 +0000
- Revision:
- 6:2c7b0e609713
- Parent:
- 5:eec419520d71
- Child:
- 8:82efd1be89ee
fixed ros params (declare in global scope) -- brian;
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 | 6:2c7b0e609713 | 25 | /********************* Helper Functions and Structures *******************/ |
bobolee1239 | 2:ff7641d822df | 26 | typedef volatile struct Controller { |
bobolee1239 | 2:ff7641d822df | 27 | volatile double rpm; // rotation speed in rpm |
bobolee1239 | 2:ff7641d822df | 28 | volatile double refRPM; // reference rpm |
bobolee1239 | 2:ff7641d822df | 29 | volatile double piOut; // PI Controller output |
bobolee1239 | 2:ff7641d822df | 30 | volatile double err; // error |
bobolee1239 | 2:ff7641d822df | 31 | volatile double ierr; // integration of error |
bobolee1239 | 2:ff7641d822df | 32 | double kp; // propotinal gain |
bobolee1239 | 2:ff7641d822df | 33 | double ki; // integrational gain |
bobolee1239 | 2:ff7641d822df | 34 | } Controller_t; |
bobolee1239 | 2:ff7641d822df | 35 | |
bobolee1239 | 2:ff7641d822df | 36 | typedef volatile struct Vehicle { |
bobolee1239 | 2:ff7641d822df | 37 | double r; // radius |
bobolee1239 | 2:ff7641d822df | 38 | double L; // distance between two wheel |
bobolee1239 | 2:ff7641d822df | 39 | volatile double V; // forward speed |
bobolee1239 | 2:ff7641d822df | 40 | volatile double W; // rotation speed |
bobolee1239 | 6:2c7b0e609713 | 41 | volatile double refV; // reference forward speed |
bobolee1239 | 6:2c7b0e609713 | 42 | volatile double refW; // reference rotation speed |
bobolee1239 | 2:ff7641d822df | 43 | } Vehicle_t; |
bobolee1239 | 2:ff7641d822df | 44 | |
bobolee1239 | 2:ff7641d822df | 45 | /* Init Structure Instance */ |
bobolee1239 | 6:2c7b0e609713 | 46 | Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 6:2c7b0e609713 | 47 | Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; |
bobolee1239 | 2:ff7641d822df | 48 | Vehicle_t car = {0.03, 0.27, 0.0, 0.0, 0.0, 0.0}; |
bobolee1239 | 2:ff7641d822df | 49 | |
bobolee1239 | 6:2c7b0e609713 | 50 | |
bobolee1239 | 6:2c7b0e609713 | 51 | void initTimer(); |
bobolee1239 | 6:2c7b0e609713 | 52 | void initPWM(); |
bobolee1239 | 6:2c7b0e609713 | 53 | void controllerTISR(); |
bobolee1239 | 6:2c7b0e609713 | 54 | void messageCallback(const geometry_msgs::Twist& recvMsg); |
bobolee1239 | 6:2c7b0e609713 | 55 | /************************************************************************/ |
bobolee1239 | 6:2c7b0e609713 | 56 | |
bobolee1239 | 6:2c7b0e609713 | 57 | /************************************************************************ |
bobolee1239 | 6:2c7b0e609713 | 58 | ** ROS Parameters Must Declare in Global Scope !!!! |
bobolee1239 | 6:2c7b0e609713 | 59 | ************************************************************************/ |
bobolee1239 | 6:2c7b0e609713 | 60 | ros::NodeHandle nh; |
bobolee1239 | 6:2c7b0e609713 | 61 | geometry_msgs::Twist vel_msg; |
bobolee1239 | 6:2c7b0e609713 | 62 | |
bobolee1239 | 6:2c7b0e609713 | 63 | ros::Publisher pub("feedback_Vel", &vel_msg); |
bobolee1239 | 6:2c7b0e609713 | 64 | ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCallback); |
bobolee1239 | 6:2c7b0e609713 | 65 | |
bobolee1239 | 6:2c7b0e609713 | 66 | /************************************************************************/ |
bobolee1239 | 6:2c7b0e609713 | 67 | /** SERIAL DEBUG |
bobolee1239 | 6:2c7b0e609713 | 68 | ************************************************************************/ |
bobolee1239 | 6:2c7b0e609713 | 69 | //Serial pc(USBTX, USBRX); |
bobolee1239 | 6:2c7b0e609713 | 70 | |
bobolee1239 | 2:ff7641d822df | 71 | int main(int argc, char* argv[]) { |
bobolee1239 | 6:2c7b0e609713 | 72 | /* SERIAL DEBUG */ |
bobolee1239 | 6:2c7b0e609713 | 73 | // pc.baud(115200); |
bobolee1239 | 6:2c7b0e609713 | 74 | |
bobolee1239 | 2:ff7641d822df | 75 | initTimer(); |
bobolee1239 | 2:ff7641d822df | 76 | initPWM(); |
bobolee1239 | 2:ff7641d822df | 77 | init_CN(); |
bobolee1239 | 0:1d023e270dfa | 78 | |
bobolee1239 | 2:ff7641d822df | 79 | nh.initNode(); |
bobolee1239 | 2:ff7641d822df | 80 | nh.subscribe(sub); |
bobolee1239 | 2:ff7641d822df | 81 | nh.advertise(pub); |
bobolee1239 | 2:ff7641d822df | 82 | |
bobolee1239 | 2:ff7641d822df | 83 | while (1) { |
bobolee1239 | 2:ff7641d822df | 84 | /************************* |
bobolee1239 | 2:ff7641d822df | 85 | ** 1. 2pi/60 = 0.1047197551 |
bobolee1239 | 2:ff7641d822df | 86 | ** 2. 2pi/60/2 = 0.05235987756 |
bobolee1239 | 2:ff7641d822df | 87 | **************************/ |
bobolee1239 | 2:ff7641d822df | 88 | car.V = (controller1.rpm + controller2.rpm) * car.r * 0.05235987756; |
bobolee1239 | 2:ff7641d822df | 89 | car.W = -1.0 * car.r * (controller1.rpm - controller2.rpm) / car.L * 0.1047197551; |
bobolee1239 | 2:ff7641d822df | 90 | |
bobolee1239 | 2:ff7641d822df | 91 | vel_msg.linear.x = car.V; |
bobolee1239 | 2:ff7641d822df | 92 | vel_msg.angular.z = car.W; |
bobolee1239 | 2:ff7641d822df | 93 | |
bobolee1239 | 2:ff7641d822df | 94 | pub.publish(&vel_msg); |
bobolee1239 | 2:ff7641d822df | 95 | |
bobolee1239 | 2:ff7641d822df | 96 | nh.spinOnce(); |
bobolee1239 | 2:ff7641d822df | 97 | |
bobolee1239 | 2:ff7641d822df | 98 | wait_ms(100); |
bobolee1239 | 0:1d023e270dfa | 99 | } |
bobolee1239 | 0:1d023e270dfa | 100 | } |
bobolee1239 | 2:ff7641d822df | 101 | |
bobolee1239 | 2:ff7641d822df | 102 | void initTimer() { |
bobolee1239 | 2:ff7641d822df | 103 | controllerTimer.attach_us(&controllerTISR, 10000.0); |
bobolee1239 | 2:ff7641d822df | 104 | } |
bobolee1239 | 2:ff7641d822df | 105 | |
bobolee1239 | 2:ff7641d822df | 106 | void initPWM() { |
bobolee1239 | 2:ff7641d822df | 107 | pwm1.period_us(50); |
bobolee1239 | 2:ff7641d822df | 108 | pwm1.write(0.5); |
bobolee1239 | 2:ff7641d822df | 109 | TIM1->CCER |= 0x4; |
bobolee1239 | 2:ff7641d822df | 110 | |
bobolee1239 | 2:ff7641d822df | 111 | pwm2.period_us(50); |
bobolee1239 | 2:ff7641d822df | 112 | pwm2.write(0.5); |
bobolee1239 | 2:ff7641d822df | 113 | TIM1->CCER |= 0x40; |
bobolee1239 | 2:ff7641d822df | 114 | } |
bobolee1239 | 2:ff7641d822df | 115 | |
bobolee1239 | 2:ff7641d822df | 116 | void controllerTISR() { |
bobolee1239 | 2:ff7641d822df | 117 | // 60.0 / 2pi = 9.549296586 |
bobolee1239 | 2:ff7641d822df | 118 | controller1.refRPM = (car.refV/car.r - car.L*car.refW/2.0/car.r) / 9.549296586; |
bobolee1239 | 2:ff7641d822df | 119 | controller2.refRPM = (car.refV/car.r + car.L*car.refW/2.0/car.r) / 9.549296586; |
bobolee1239 | 2:ff7641d822df | 120 | |
bobolee1239 | 6:2c7b0e609713 | 121 | // pc.printf("%.2f, %.2f", controller1.refRPM, controller2.refRPM); |
bobolee1239 | 6:2c7b0e609713 | 122 | |
bobolee1239 | 2:ff7641d822df | 123 | /********* MOTOR 1 ******************/ |
bobolee1239 | 2:ff7641d822df | 124 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 2:ff7641d822df | 125 | controller1.rpm = (double)wheelState1.numStateChange * 2.232142857; |
bobolee1239 | 2:ff7641d822df | 126 | wheelState1.numStateChange = 0; |
bobolee1239 | 2:ff7641d822df | 127 | |
bobolee1239 | 2:ff7641d822df | 128 | controller1.err = controller1.refRPM - controller1.rpm; |
bobolee1239 | 2:ff7641d822df | 129 | controller1.ierr += Ts*controller1.err; |
bobolee1239 | 2:ff7641d822df | 130 | |
bobolee1239 | 2:ff7641d822df | 131 | if (controller1.ierr > 50.0) { |
bobolee1239 | 2:ff7641d822df | 132 | controller1.ierr = 50.0; |
bobolee1239 | 2:ff7641d822df | 133 | } else if (controller1.ierr < -50.0) { |
bobolee1239 | 2:ff7641d822df | 134 | controller1.ierr = -50.0; |
bobolee1239 | 2:ff7641d822df | 135 | } |
bobolee1239 | 2:ff7641d822df | 136 | controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr; |
bobolee1239 | 2:ff7641d822df | 137 | |
bobolee1239 | 2:ff7641d822df | 138 | if (controller1.piOut > 0.5) { |
bobolee1239 | 2:ff7641d822df | 139 | controller1.piOut = 0.5; |
bobolee1239 | 2:ff7641d822df | 140 | } else if (controller1.piOut < -0.5) { |
bobolee1239 | 2:ff7641d822df | 141 | controller1.piOut = -0.5; |
bobolee1239 | 2:ff7641d822df | 142 | } |
bobolee1239 | 2:ff7641d822df | 143 | |
bobolee1239 | 2:ff7641d822df | 144 | pwm1.write(controller1.piOut + 0.5); |
bobolee1239 | 2:ff7641d822df | 145 | TIM1->CCER |= 0x4; |
bobolee1239 | 2:ff7641d822df | 146 | |
bobolee1239 | 2:ff7641d822df | 147 | /********* MOTOR 2 ******************/ |
bobolee1239 | 2:ff7641d822df | 148 | // 100/48*60/56 = 2.232142857 |
bobolee1239 | 2:ff7641d822df | 149 | controller2.rpm = (double)wheelState2.numStateChange * 2.232142857; |
bobolee1239 | 2:ff7641d822df | 150 | wheelState2.numStateChange = 0; |
bobolee1239 | 2:ff7641d822df | 151 | |
bobolee1239 | 2:ff7641d822df | 152 | controller2.err = controller2.refRPM - controller2.rpm; |
bobolee1239 | 2:ff7641d822df | 153 | controller2.ierr += Ts*controller2.err; |
bobolee1239 | 2:ff7641d822df | 154 | |
bobolee1239 | 2:ff7641d822df | 155 | if (controller2.ierr > 50.0) { |
bobolee1239 | 2:ff7641d822df | 156 | controller2.ierr = 50.0; |
bobolee1239 | 2:ff7641d822df | 157 | } else if (controller2.ierr < -50.0) { |
bobolee1239 | 2:ff7641d822df | 158 | controller2.ierr = -50.0; |
bobolee1239 | 2:ff7641d822df | 159 | } |
bobolee1239 | 6:2c7b0e609713 | 160 | controller2.piOut = -(controller2.kp*controller2.err + controller2.ki*controller2.ierr); |
bobolee1239 | 2:ff7641d822df | 161 | |
bobolee1239 | 2:ff7641d822df | 162 | if (controller2.piOut > 0.5) { |
bobolee1239 | 2:ff7641d822df | 163 | controller2.piOut = 0.5; |
bobolee1239 | 2:ff7641d822df | 164 | } else if (controller2.piOut < -0.5) { |
bobolee1239 | 2:ff7641d822df | 165 | controller2.piOut = -0.5; |
bobolee1239 | 2:ff7641d822df | 166 | } |
bobolee1239 | 2:ff7641d822df | 167 | |
bobolee1239 | 6:2c7b0e609713 | 168 | pwm2.write(controller2.piOut + 0.5); |
bobolee1239 | 2:ff7641d822df | 169 | TIM1->CCER |= 0x40; |
bobolee1239 | 2:ff7641d822df | 170 | } |
bobolee1239 | 2:ff7641d822df | 171 | |
bobolee1239 | 2:ff7641d822df | 172 | void messageCallback(const geometry_msgs::Twist& recvMsg) { |
bobolee1239 | 6:2c7b0e609713 | 173 | // pc.printf("receiving message ...\n"); |
bobolee1239 | 2:ff7641d822df | 174 | car.refV = 0.25 * recvMsg.linear.x; |
bobolee1239 | 2:ff7641d822df | 175 | car.refW = recvMsg.angular.z; |
bobolee1239 | 2:ff7641d822df | 176 | } |