mobile_robot_tea / Mbed 2 deprecated mobile_robot_lab3_rosserial

lab3

Dependencies:   mbed ros_lib_kinetic hallsensor_software_decoder

main.cpp@2:ff7641d822df, 2019-03-27 (annotated)

Committer:
bobolee1239
Date:
Wed Mar 27 03:00:26 2019 +0000
Revision:
2:ff7641d822df
Parent:
0:1d023e270dfa
Child:
3:a308ddde9078
modified version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bobolee1239 2:ff7641d822df 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 }