lab3
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
Diff: main.cpp
- Revision:
- 2:ff7641d822df
- Parent:
- 0:1d023e270dfa
- Child:
- 3:a308ddde9078
diff -r 8917d19c9921 -r ff7641d822df main.cpp --- a/main.cpp Wed Mar 27 01:27:14 2019 +0000 +++ b/main.cpp Wed Mar 27 03:00:26 2019 +0000 @@ -1,24 +1,164 @@ +// 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" - -AnalogIn analog_value(A0); - -DigitalOut led(LED1); +#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); -int main() { - float meas; - - printf("\nAnalogIn example\n"); +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(); - while(1) { - meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0) - meas = meas * 3300; // Change the value to be in the 0 to 3300 range - printf("measure = %.0f mV\n", meas); - if (meas > 2000) { // If the value is greater than 2V then switch the LED on - led = 1; - } - else { - led = 0; - } - wait(0.2); // 200 ms + /************************ 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; +}