Diff: machine_ps3.h

Revision:

26:760f1bce8214

Parent:

9:f9a0c7ca640f

Child:

30:cd344beb415d

--- a/machine_ps3.h	Sun Nov 08 04:26:26 2015 +0000
+++ b/machine_ps3.h	Sun Nov 08 10:56:27 2015 +0000
@@ -7,11 +7,6 @@
 #define MACHINE_H
 
 /**
- * Functions prototype.
- */
-
-
-/**
  * Includes
  */
 #include "mbed.h"
@@ -30,6 +25,22 @@
 /**
  * Functions.
  */
+/***called by Com***/
+void Call(){
+    mesure_state();
+    move_following();
+    if(spcount<speed){
+        spcount+=speed/100.0;
+        targ_velocity=spcount;
+    }
+    if(dpcount>0.0){
+        dpcount-=speed/100.0;
+        targ_velocity=dpcount;
+    }
+    mesureSwing();
+    mesure_state();
+    if(enableSwing) swingFollowing();
+}
 
 /***The function is motors initialize.***/
 inline void initializeMotors()
@@ -87,8 +98,71 @@
     y                   += dy;
 }
 
+/***The function is PID controller initialize.***/
+inline void initializeControllers()
+{
+    velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
+    direction_controller.setInputLimits(-PI, PI); //x2
+
+    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
+    velocity_controller.setOutputLimits(0.0, 1.0);
+    direction_controller.setOutputLimits(-1.0, 1.0);
+
+    //set bias. 初期値
+    velocity_controller.setBias(0.0);
+    direction_controller.setBias(0.0);
+
+    //set mode.
+    velocity_controller.setMode(AUTO_MODE);
+    direction_controller.setMode(AUTO_MODE);
+}
+
+/***The function is following move speed.***/
+inline void velocity_following()
+{
+    velocity_controller.setSetPoint((float)targ_velocity);
+    velocity_controller.setProcessValue((float)velocity);
+    x1 = (double)velocity_controller.compute();
+}
+
+inline void arg_following()
+{
+    direction_controller.setSetPoint((float)targ_sita);
+    direction_controller.setProcessValue((float)sita);
+    x2 = (double)direction_controller.compute();
+}
+
+inline void move_following()
+{
+    velocity_following();
+    arg_following();
+    
+    if(flagf==0){
+        if(x2>0.0){
+            Vr = -x1 + x2;
+            Vl = -x1;
+        }
+        else{
+            Vr = -x1;
+            Vl = -x1 - x2;
+        }
+    }
+    else if(flagf==1){
+        if(x2>0.0){
+            Vr = x1;
+            Vl = x1 - x2;
+        }
+        else{
+            Vr = x1 + x2;
+            Vl = x1;
+        }
+    }
+    Move_r( ( float ) Vr );
+    Move_l( ( float ) Vl );
+}
+
+/***The function reset state.***/
 void resetState(){
-    wait(0.1);
 #ifdef BLUE
     x=0.0;
     y=0.0;