Diff: main.cpp

Revision:

2:ff7641d822df

Parent:

0:1d023e270dfa

Child:

3:a308ddde9078

--- 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;
+}