main.cpp

00001 //  Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen
00002 /***************************************************************
00003  **     MOBILE ROBOT LAB3 GROUP 4 [Vehicle Motion Control]
00004  **
00005  **  - Members  : Tsung-Han Lee, Tommy, Skyler
00006  **  - NOTE     : Modified from sample code provided
00007  **               by NTHU Dynamic Systems and Control Lab
00008  ** 
00009  ***************************************************************/
00010 #include <ros.h>
00011 #include <geometry_msgs/Twist.h>
00012 #include "mbed.h"
00013 #include "hallsensor_software_decoder.h"
00014 
00015 #define Ts 0.01
00016 
00017 Ticker controllerTimer;
00018 
00019 /* declare pwm pin */
00020 PwmOut pwm1(D7);
00021 PwmOut pwm1n(D11);
00022 PwmOut pwm2(D8);
00023 PwmOut pwm2n(A3);
00024 
00025 /********************* Helper Functions and Structures *******************/
00026 typedef volatile struct Controller {
00027     volatile double rpm;        // rotation speed in rpm
00028     volatile double refRPM;     // reference rpm
00029     volatile double piOut;      // PI Controller output
00030     volatile double err;        // error
00031     volatile double ierr;       // integration of error
00032     double kp;                  // propotinal gain
00033     double ki;                  // integrational gain
00034 } Controller_t;
00035 
00036 typedef volatile struct Vehicle {
00037     double r;                   // radius
00038     double L;                   // distance between two wheel
00039     volatile double V;          // forward speed
00040     volatile double W;          // rotation speed
00041     volatile double refV;       // reference forward speed
00042     volatile double refW;       // reference rotation speed
00043 } Vehicle_t;
00044 
00045 /* Init Structure Instance */
00046 Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
00047 Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
00048 Vehicle_t    car         = {0.03, 0.27, 0.0, 0.0, 0.0, 0.0};  
00049 
00050 
00051 void initTimer();
00052 void initPWM();
00053 void controllerTISR();
00054 void messageCallback(const geometry_msgs::Twist& recvMsg);
00055 /************************************************************************/
00056 
00057 /************************************************************************
00058  ** ROS Parameters Must Declare in Global Scope !!!!
00059  ************************************************************************/
00060 ros::NodeHandle nh;
00061 geometry_msgs::Twist vel_msg;
00062     
00063 ros::Publisher pub("feedback_Vel", &vel_msg);
00064 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCallback);
00065 
00066 /************************************************************************/
00067 /** SERIAL DEBUG 
00068  ************************************************************************/
00069 //Serial pc(USBTX, USBRX);
00070 
00071 int main(int argc, char* argv[]) {
00072     /* SERIAL DEBUG */
00073 //    pc.baud(115200);
00074     
00075     initTimer();
00076     initPWM();
00077     init_CN();
00078     
00079     nh.initNode();
00080     nh.subscribe(sub);
00081     nh.advertise(pub);
00082 
00083     while (1) {
00084         /*************************
00085          **  1. 2pi/60      = 0.1047197551
00086          **  2. 2pi/60/2    = 0.05235987756
00087          **************************/
00088         car.V = (controller1.rpm + controller2.rpm) * car.r * 0.05235987756;
00089         car.W = -1.0 * car.r * (controller1.rpm - controller2.rpm) / car.L * 0.1047197551;
00090         
00091         vel_msg.linear.x    = car.V;
00092         vel_msg.angular.z   = car.W;
00093         
00094         pub.publish(&vel_msg);
00095         
00096         nh.spinOnce();
00097         
00098         wait_ms(100);  
00099     }
00100 }
00101 
00102 void initTimer() {
00103     controllerTimer.attach_us(&controllerTISR, 10000.0);    
00104 }
00105 
00106 void initPWM() {
00107     pwm1.period_us(50);
00108     pwm1.write(0.5);
00109     TIM1->CCER |= 0x4;
00110     
00111     pwm2.period_us(50);
00112     pwm2.write(0.5);
00113     TIM1->CCER |= 0x40;   
00114 }
00115 
00116 void controllerTISR() {
00117     // 60.0 / 2pi = 9.549296586
00118     controller1.refRPM = (car.refV - car.L*car.refW/2.0) * 9.549296586 / car.r;
00119     controller2.refRPM = (car.refV + car.L*car.refW/2.0) * 9.549296586 / car.r;
00120     
00121 //    pc.printf("%.2f, %.2f", controller1.refRPM, controller2.refRPM);
00122     
00123     /********* MOTOR 1 ******************/
00124     // 100/48*60/56 = 2.232142857
00125     controller1.rpm            = (double)wheelState1.numStateChange * 2.232142857;
00126     wheelState1.numStateChange = 0;
00127     
00128     controller1.err   = controller1.refRPM - controller1.rpm;
00129     controller1.ierr += Ts*controller1.err;
00130     
00131     if (controller1.ierr > 50.0) {
00132         controller1.ierr = 50.0;    
00133     } else if (controller1.ierr < -50.0) {
00134         controller1.ierr = -50.0;    
00135     }
00136     controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr;
00137 
00138     if (controller1.piOut > 0.5) {
00139         controller1.piOut = 0.5;    
00140     } else if (controller1.piOut < -0.5) {
00141         controller1.piOut = -0.5;    
00142     }
00143     
00144     pwm1.write(controller1.piOut + 0.5);
00145     TIM1->CCER |= 0x4;
00146     
00147     /********* MOTOR 2 ******************/
00148     // 100/48*60/56 = 2.232142857
00149     controller2.rpm            = (double)wheelState2.numStateChange * 2.232142857;
00150     wheelState2.numStateChange = 0;
00151     
00152     controller2.err   = controller2.refRPM - controller2.rpm;
00153     controller2.ierr += Ts*controller2.err;
00154     
00155     if (controller2.ierr > 50.0) {
00156         controller2.ierr = 50.0;    
00157     } else if (controller2.ierr < -50.0) {
00158         controller2.ierr = -50.0;    
00159     }
00160     controller2.piOut = -(controller2.kp*controller2.err + controller2.ki*controller2.ierr);
00161 
00162     if (controller2.piOut > 0.5) {
00163         controller2.piOut = 0.5;    
00164     } else if (controller2.piOut < -0.5) {
00165         controller2.piOut = -0.5;    
00166     }
00167     
00168     pwm2.write(controller2.piOut + 0.5);
00169     TIM1->CCER |= 0x40;
00170 }
00171 
00172 void messageCallback(const geometry_msgs::Twist& recvMsg) {
00173 //    pc.printf("receiving message ...\n");
00174     car.refV = 0.25 * recvMsg.linear.x;
00175     car.refW = recvMsg.angular.z;
00176 }