control vehicle motion
Dependencies: mbed ros_lib_kinetic hallsensor_software_decoder
Revision 0:f044e2e055ff, committed 2019-04-25
- Comitter:
- bobolee1239
- Date:
- Thu Apr 25 08:42:08 2019 +0000
- Commit message:
- first commit;
Changed in this revision
diff -r 000000000000 -r f044e2e055ff hallsensor_software_decoder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hallsensor_software_decoder.lib Thu Apr 25 08:42:08 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/bobolee1239/code/hallsensor_software_decoder/#b6d81c106fd1
diff -r 000000000000 -r f044e2e055ff main.cpp
--- /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;
+}
diff -r 000000000000 -r f044e2e055ff mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 25 08:42:08 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r f044e2e055ff ros_lib_kinetic.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Thu Apr 25 08:42:08 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f