Brian Lee / Mbed 2 deprecated mobile-robot-vehicle-control

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?

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