lab3
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
main.cpp
- Committer:
- bobolee1239
- Date:
- 2019-03-27
- Revision:
- 3:a308ddde9078
- Parent:
- 2:ff7641d822df
- Child:
- 5:eec419520d71
File content as of revision 3:a308ddde9078:
// 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 ****************/ void initTimer(); void initPWM(); void controllerTISR(); void messageCallback(const geometry_msgs::Twist& recvMsg); typedef volatile struct Controller { volatile double rpm; // rotation speed in rpm volatile double refRPM; // reference rpm volatile double pwmDuty; // duty cycle of pwm 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 refV; // reference forward speed volatile double refW; // reference forward speed volatile double V; // forward speed volatile double W; // rotation speed } Vehicle_t; /******************************************************************/ /* Init Structure Instance */ Controller_t controller1 = {0.0, 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.0, 0.008, 0.02}; Vehicle_t car = {0.03, 0.27, 0.0, 0.0, 0.0, 0.0}; int main(int argc, char* argv[]) { initTimer(); initPWM(); init_CN(); /************************ ROS Parameters ***************************/ ros::NodeHandle nh; geometry_msgs::Twist vel_msg; ros::Publisher pub("feedback_Vel", &vel_msg); ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCallback); /*******************************************************************/ 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.r - car.L*car.refW/2.0/car.r) / 9.549296586; controller2.refRPM = (car.refV/car.r + car.L*car.refW/2.0/car.r) / 9.549296586; /********* 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; } pwm1.write(controller2.piOut + 0.5); TIM1->CCER |= 0x40; } void messageCallback(const geometry_msgs::Twist& recvMsg) { car.refV = 0.25 * recvMsg.linear.x; car.refW = recvMsg.angular.z; }