Diff: main.cpp

Revision:

1:47a1aaf98baa

Parent:

0:b124e4e3a1e4

--- a/main.cpp	Wed Mar 13 06:26:51 2019 +0000
+++ b/main.cpp	Fri Mar 29 05:06:08 2019 +0000
@@ -1,165 +1,125 @@
+//  Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen
+/***************************************************************
+ **     MOBILE ROBOT LAB2 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 "mbed.h"
 #include "hallsensor_software_decoder.h"
 
-#define Ts 0.01f    // f to specify float type
+#define Ts 0.01
 
-Ticker timer1;
+Ticker controllerTimer;
 
+/* declare pwm pin */
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
 PwmOut pwm2n(A3);
 
-void init_TIMER();
-void init_PWM(); 
-void timer1_ITR();
-
-/* wheel 1 */
-float rotation_speed_1  = 0.0;
-float pwm1_duty         = 0.5;
-double PI_out_1         = 0.0;
-float err_1             = 0.0;
-float ierr_1            = 0.0;
+/********************* Helper Functions and Structures *******************/
+typedef volatile struct Controller {
+    volatile double rpm;        // rotation speed in 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;
 
-/* wheel 2 */
-float rotation_speed_2  = 0.0;
-float pwm2_duty         = 0.5;
-float PI_out_2          = 0.0;
-float err_2             = 0.0;
-float ierr_2            = 0.0;
+/* Init Structure Instance */
+Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
+Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
 
-float r = 0.03;                     // wheel radius (m)
-float L = 0.27;                     // car width (m)
-
-float rotation_speed_ref_1 = 25.0;
-float rotation_speed_ref_2 = -25.0;
-
-float V_ref = 0.0;                  // (m/s) max: 0.25
-float V     = 0.0;
-float W_ref = 0.0;                  // (rad/s)
-float W     = 0.0;
+volatile double refRPM1  = 25.0;      // reference rpm1
+volatile double refRPM2  = -25.0;      // reference rpm2
 
 
-float Kp    = 0.008;
-float Ki    = 0.02;
+void initTimer();
+void initPWM();
+void controllerTISR();
+/************************************************************************/
 
+/* SERIAL DEBUG */
 //Serial pc(USBTX, USBRX);
 
-
-int main()
-{
-    //pc.baud(9600);
-    init_TIMER();
-    init_PWM();
+int main(int argc, char* argv[]) {
+    /* SERIAL DEBUG */
+//    pc.baud(115200);
+    
+    initTimer();
+    initPWM();
     init_CN();
-        
-    while(1) {
-        V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f;
-        W = (-2.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f;
-    /*    pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2);
-        pc.printf("V = %f, W = %f\r\n", V,W);*/
-        
-        wait_ms(100);
+    
+    while (1) {
+//        pc.printf("%.2f, %.2f \r\n", controller1.rpm, controller2.rpm);
+        wait_ms(100);  
     }
 }
 
-
-
-void init_TIMER() {
-    /********************************************************
-     * function to be attached (timer1_ITR) and 
-     * the interval (10000 micro-seconds) = 0.01 second
-     *********************************************************/
-    timer1.attach_us(&timer1_ITR, 10000.0); 
+void initTimer() {
+    controllerTimer.attach_us(&controllerTISR, 10000.0);    
 }
 
-
-void init_PWM(){
-    /* setting pwm width */
-    
+void initPWM() {
     pwm1.period_us(50);
     pwm1.write(0.5);
     TIM1->CCER |= 0x4;
-
+    
     pwm2.period_us(50);
     pwm2.write(0.5);
-    TIM1->CCER |= 0x40;
+    TIM1->CCER |= 0x40;   
 }
 
-
-void timer1_ITR()
-{
+void controllerTISR() {        
+    /********* MOTOR 1 ******************/
+    // 100/48*60/56 = 2.232142857
+    controller1.rpm            = (double)wheelState1.numStateChange * 2.232142857;
+    wheelState1.numStateChange = 0;
     
-    //rotation_speed_ref_1 = (V_ref / r - L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
-    //rotation_speed_ref_2 = (V_ref / r + L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
-    
-    
-    /********** MOTOR1 **********/
+    controller1.err   = refRPM1 - controller1.rpm;
+    controller1.ierr += Ts*controller1.err;
     
-    //measure
-    rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
-    // reset
-    speed_count_1 = 0;
-    
-    /************ PI controller for motor1 *************/
-    
-    err_1 = rotation_speed_ref_1 - rotation_speed_1;
-    ierr_1 = ierr_1 + Ts * err_1;
-    
-    // restrict integrating part
-    if(ierr_1 > 50.0f){
-        ierr_1 = 50.0;
-    } else if(ierr_1 < -50.0f){
-        ierr_1 = -50.0;
+    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;    
     }
     
-    PI_out_1 = (Kp * err_1 + Ki * ierr_1);
-    
-    // staturation
-    if(PI_out_1 >= 0.5f){
-        PI_out_1 = 0.5;
-    } else if(PI_out_1 <= -0.5f){
-        PI_out_1 = -0.5; 
-    }
-    /*************************************************/
-    
-    /* output to acuator */
-    pwm1_duty = PI_out_1 + 0.5f;   
-    pwm1.write(pwm1_duty);
+    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;
     
-   /********* MOTOR2 ***********/
-  
-
-    rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
-    speed_count_2 = 0;
-
-    /************* PI controller for motor2 ****************/
-    err_2 = rotation_speed_ref_2 - rotation_speed_2;
-    ierr_2 = ierr_2 + Ts * err_2;
+    controller2.err   = refRPM2 - controller2.rpm;
+    controller2.ierr += Ts*controller2.err;
     
-    // restrict integration output
-    if(ierr_2 > 50.0f){
-        ierr_2 = 50.0;
-    } else if(ierr_2 < -50.0f){
-        ierr_2 = -50.0;
+    if (controller2.ierr > 50.0) {
+        controller2.ierr = 50.0;    
+    } else if (controller2.ierr < -50.0) {
+        controller2.ierr = -50.0;    
     }
-    PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ;
-  
-    // staturation
-    if(PI_out_2 >= 0.5f){
-        PI_out_2 = 0.5;
-    } else if(PI_out_2 <= -0.5f) {
-        PI_out_2 = -0.5;
+    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;    
     }
-    /******************************************************/
-    /* output to acuator */
-    pwm2_duty = PI_out_2 + 0.5f;
-    pwm2.write(pwm2_duty);
+    
+    pwm2.write(-controller2.piOut + 0.5);
     TIM1->CCER |= 0x40;
-    
-
-}
\ No newline at end of file
+}