control vehicle motion
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
Diff: main.cpp
- Revision:
- 0:f044e2e055ff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 25 08:42:08 2019 +0000 @@ -0,0 +1,176 @@ +// Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen +/*************************************************************** + ** MOBILE ROBOT LAB3 GROUP 4 [Vehicle Motion Control] + ** + ** - Members : Tsung-Han Lee, Tommy, Skyler + ** - NOTE : Modified from sample code provided + ** by NTHU Dynamic Systems and Control Lab + ** + ***************************************************************/ +#include <ros.h> +#include <geometry_msgs/Twist.h> +#include "mbed.h" +#include "hallsensor_software_decoder.h" + +#define Ts 0.01 + +Ticker controllerTimer; + +/* declare pwm pin */ +PwmOut pwm1(D7); +PwmOut pwm1n(D11); +PwmOut pwm2(D8); +PwmOut pwm2n(A3); + +/********************* Helper Functions and Structures *******************/ +typedef volatile struct Controller { + volatile double rpm; // rotation speed in rpm + volatile double refRPM; // reference rpm + volatile double piOut; // PI Controller output + volatile double err; // error + volatile double ierr; // integration of error + double kp; // propotinal gain + double ki; // integrational gain +} Controller_t; + +typedef volatile struct Vehicle { + double r; // radius + double L; // distance between two wheel + volatile double V; // forward speed + volatile double W; // rotation speed + volatile double refV; // reference forward speed + volatile double refW; // reference rotation speed +} Vehicle_t; + +/* Init Structure Instance */ +Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; +Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02}; +Vehicle_t car = {0.03, 0.27, 0.0, 0.0, 0.0, 0.0}; + + +void initTimer(); +void initPWM(); +void controllerTISR(); +void messageCallback(const geometry_msgs::Twist& recvMsg); +/************************************************************************/ + +/************************************************************************ + ** ROS Parameters Must Declare in Global Scope !!!! + ************************************************************************/ +ros::NodeHandle nh; +geometry_msgs::Twist vel_msg; + +ros::Publisher pub("feedback_Vel", &vel_msg); +ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCallback); + +/************************************************************************/ +/** SERIAL DEBUG + ************************************************************************/ +//Serial pc(USBTX, USBRX); + +int main(int argc, char* argv[]) { + /* SERIAL DEBUG */ +// pc.baud(115200); + + initTimer(); + initPWM(); + init_CN(); + + nh.initNode(); + nh.subscribe(sub); + nh.advertise(pub); + + while (1) { + /************************* + ** 1. 2pi/60 = 0.1047197551 + ** 2. 2pi/60/2 = 0.05235987756 + **************************/ + car.V = (controller1.rpm + controller2.rpm) * car.r * 0.05235987756; + car.W = -1.0 * car.r * (controller1.rpm - controller2.rpm) / car.L * 0.1047197551; + + vel_msg.linear.x = car.V; + vel_msg.angular.z = car.W; + + pub.publish(&vel_msg); + + nh.spinOnce(); + + wait_ms(100); + } +} + +void initTimer() { + controllerTimer.attach_us(&controllerTISR, 10000.0); +} + +void initPWM() { + pwm1.period_us(50); + pwm1.write(0.5); + TIM1->CCER |= 0x4; + + pwm2.period_us(50); + pwm2.write(0.5); + TIM1->CCER |= 0x40; +} + +void controllerTISR() { + // 60.0 / 2pi = 9.549296586 + controller1.refRPM = (car.refV - car.L*car.refW/2.0) * 9.549296586 / car.r; + controller2.refRPM = (car.refV + car.L*car.refW/2.0) * 9.549296586 / car.r; + +// pc.printf("%.2f, %.2f", controller1.refRPM, controller2.refRPM); + + /********* MOTOR 1 ******************/ + // 100/48*60/56 = 2.232142857 + controller1.rpm = (double)wheelState1.numStateChange * 2.232142857; + wheelState1.numStateChange = 0; + + controller1.err = controller1.refRPM - controller1.rpm; + controller1.ierr += Ts*controller1.err; + + if (controller1.ierr > 50.0) { + controller1.ierr = 50.0; + } else if (controller1.ierr < -50.0) { + controller1.ierr = -50.0; + } + controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr; + + if (controller1.piOut > 0.5) { + controller1.piOut = 0.5; + } else if (controller1.piOut < -0.5) { + controller1.piOut = -0.5; + } + + pwm1.write(controller1.piOut + 0.5); + TIM1->CCER |= 0x4; + + /********* MOTOR 2 ******************/ + // 100/48*60/56 = 2.232142857 + controller2.rpm = (double)wheelState2.numStateChange * 2.232142857; + wheelState2.numStateChange = 0; + + controller2.err = controller2.refRPM - controller2.rpm; + controller2.ierr += Ts*controller2.err; + + if (controller2.ierr > 50.0) { + controller2.ierr = 50.0; + } else if (controller2.ierr < -50.0) { + controller2.ierr = -50.0; + } + controller2.piOut = -(controller2.kp*controller2.err + controller2.ki*controller2.ierr); + + if (controller2.piOut > 0.5) { + controller2.piOut = 0.5; + } else if (controller2.piOut < -0.5) { + controller2.piOut = -0.5; + } + + pwm2.write(controller2.piOut + 0.5); + TIM1->CCER |= 0x40; +} + +void messageCallback(const geometry_msgs::Twist& recvMsg) { +// pc.printf("receiving message ...\n"); + car.refV = recvMsg.linear.x; + car.refW = recvMsg.angular.z; +}