Diff: machine_ps3.h

Revision:

2:cf8ca6742db9

Parent:

1:a89e2a604dcf

Child:

3:5266af49834f

Child:

4:51d87d2b698c

--- a/machine_ps3.h	Mon Aug 31 00:25:25 2015 +0000
+++ b/machine_ps3.h	Mon Aug 31 04:10:00 2015 +0000
@@ -1,5 +1,5 @@
 /**
- * This library is included main.cpp. 
+ * This library is included main.cpp.
  * class "machine" whitch machine functions are structured.
  */
 
@@ -39,7 +39,7 @@
  * include function header files.
  */
 #include "communicate.h"
- 
+
 /**
  * Defines
  */
@@ -48,7 +48,7 @@
 /**
  * Set functions.
  */
- 
+
 /***PID Controller***/
 PID move_r_controller(MOVE_R_Kc, MOVE_R_TAUi, MOVE_R_TAUd, RATE);
 PID move_l_controller(MOVE_L_Kc, MOVE_L_TAUi, MOVE_L_TAUd, RATE);
@@ -76,57 +76,59 @@
  */
 
 /***The function is motors initialize.***/
-inline void initializeMotors(){
+inline void initializeMotors()
+{
     Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
     Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
     Motor_swing_pwm.period_us(SWING_PERIOD_US);
 }
 
 /***The function is PID controller initialize.***/
-inline void initializeControllers(){    
+inline void initializeControllers()
+{
 
     move_r_controller.setInputLimits(MOVE_R_INPUT_LIMIT_BOTTOM, MOVE_R_INPUT_LIMIT_TOP);
     move_l_controller.setInputLimits(MOVE_L_INPUT_LIMIT_BOTTOM, MOVE_L_INPUT_LIMIT_TOP);
     swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
-    
+
     //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
     move_r_controller.setOutputLimits(MOVE_R_OUTPUT_LIMIT_BOTTOM, MOVE_R_OUTPUT_LIMIT_TOP);
     move_l_controller.setOutputLimits(MOVE_L_OUTPUT_LIMIT_BOTTOM, MOVE_L_OUTPUT_LIMIT_TOP);
     swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
-    
+
     //set bias.
     move_r_controller.setBias(MOVE_R_BIAS);
     move_l_controller.setBias(MOVE_L_BIAS);
     swing_controller.setBias(SWING_BIAS);
-    
+
     //set mode.
     move_r_controller.setMode(AUTO_MODE);
     move_l_controller.setMode(AUTO_MODE);
     swing_controller.setMode(AUTO_MODE);
 }
 
-/***The function is Move on field.***/ 
+/***The function is Move on field.***/
 //Right
-void Move_r(float speed1){ 
-    if(speed1<0){
+void Move_r(float speed1)
+{
+    if(speed1<0) {
         Move_r_Motor_CCW = 1;
         Move_r_Motor_CW = 0;
         Move_r_Motor_PWM = abs(speed1);
-    }
-    else{
+    } else {
         Move_r_Motor_CCW = 0;
         Move_r_Motor_CW = 1;
         Move_r_Motor_PWM = speed1;
     }
 }
 //Left
-void Move_l(float speed2){ 
-    if(speed2<0){
+void Move_l(float speed2)
+{
+    if(speed2<0) {
         Move_l_Motor_CCW = 1;
         Move_l_Motor_CW = 0;
         Move_l_Motor_PWM = abs(speed2);
-    }
-    else{
+    } else {
         Move_l_Motor_CCW = 0;
         Move_l_Motor_CW = 1;
         Move_l_Motor_PWM = speed2;
@@ -134,21 +136,24 @@
 }
 
 /***Caluculate move velocity.***/
-inline void mesure_move_r_velocity(){
+inline void mesure_move_r_velocity()
+{
     Pulses_move_r = (double)Move_r_sense.getPulses();
     move_r_velocity = Pulses_move_r - PrefPulses_move_r;
     //move_r_velocity += (1.0-alpha)*Prefmove_r_velocity;
     //Prefmove_r_velocity = move_r_velocity;
     PrefPulses_move_r = Pulses_move_r;
 }
-inline void mesure_move_l_velocity(){
+inline void mesure_move_l_velocity()
+{
     Pulses_move_l = (double)Move_l_sense.getPulses();
     move_l_velocity = Pulses_move_l - PrefPulses_move_l;
     //move_l_velocity += (1.0-alpha)*Prefmove_l_velocity;
     //Prefmove_l_velocity = move_l_velocity;
     PrefPulses_move_l = Pulses_move_l;
 }
-inline void mesure_swing_velocity(){
+inline void mesure_swing_velocity()
+{
     Pulses_swing = (double)Swing_speed_sens.getPulses();
     swing_velocity = Pulses_swing - PrefPulses_swing;
     PrefPulses_swing = Pulses_swing;
@@ -158,19 +163,22 @@
 float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
 short r=0,l=0;
 
-inline void Move_r_speed_following(){
+inline void Move_r_speed_following()
+{
     move_r_controller.setSetPoint((float)targ_move_r_velocity);
     move_r_controller.setProcessValue((float)move_r_velocity);
     cont_move_r = move_r_controller.compute();
     Move_r(cont_move_r);
 }
-inline void Move_l_speed_following(){
+inline void Move_l_speed_following()
+{
     move_l_controller.setSetPoint((float)targ_move_l_velocity);
     move_l_controller.setProcessValue((float)move_l_velocity);
     cont_move_l = move_l_controller.compute();
     Move_l(cont_move_l);
 }
-inline void Swing_speed_following(){
+inline void Swing_speed_following()
+{
     swing_controller.setSetPoint((float)targ_swing_velocity);
     swing_controller.setProcessValue((float)swing_velocity);
     cont_swing = swing_controller.compute();
@@ -178,8 +186,9 @@
 }
 
 /***Emergency stop.***/
-void Emergency_toggle(){
-    if(edge){
+void Emergency_toggle()
+{
+    if(edge) {
         edge=0;
         if(toggle) toggle=0,Emergency_stop=0;
         else toggle=1,Emergency_stop=1;