Diff: main.cpp

Revision:

6:2c7b0e609713

Parent:

5:eec419520d71

Child:

8:82efd1be89ee

--- a/main.cpp	Wed Mar 27 07:13:40 2019 +0000
+++ b/main.cpp	Fri Mar 29 05:05:23 2019 +0000
@@ -22,16 +22,10 @@
 PwmOut pwm2(D8);
 PwmOut pwm2n(A3);
 
-/****************** Helper Functions and Structures ****************/
-void initTimer();
-void initPWM();
-void controllerTISR();
-void messageCallback(const geometry_msgs::Twist& recvMsg);
-
+/********************* Helper Functions and Structures *******************/
 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
@@ -42,31 +36,46 @@
 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
+    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.0, 0.008, 0.02};
-Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
+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();
     
-    /************************ 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);
@@ -109,6 +118,8 @@
     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;
     
+//    pc.printf("%.2f, %.2f", controller1.refRPM, controller2.refRPM);
+    
     /********* MOTOR 1 ******************/
     // 100/48*60/56 = 2.232142857
     controller1.rpm            = (double)wheelState1.numStateChange * 2.232142857;
@@ -146,7 +157,7 @@
     } else if (controller2.ierr < -50.0) {
         controller2.ierr = -50.0;    
     }
-    controller2.piOut = controller2.kp*controller2.err + controller2.ki*controller2.ierr;
+    controller2.piOut = -(controller2.kp*controller2.err + controller2.ki*controller2.ierr);
 
     if (controller2.piOut > 0.5) {
         controller2.piOut = 0.5;    
@@ -154,11 +165,12 @@
         controller2.piOut = -0.5;    
     }
     
-    pwm2.write(-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 = 0.25 * recvMsg.linear.x;
     car.refW = recvMsg.angular.z;
 }