Diff: machine_ps3.h

Revision:

0:bd4719e15f7e

Child:

1:a89e2a604dcf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/machine_ps3.h	Sun Aug 30 10:49:16 2015 +0000
@@ -0,0 +1,214 @@
+/**
+ * This library is included main.cpp. 
+ * class "machine" whitch machine functions are structured.
+ */
+
+#ifndef MACHINE_H
+#define MACHINE_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "QEI.h"
+#include "PID.h"
+#include "PinDefined_and_Setting_ps3.h"
+#include "Parameters_ps3.h"
+
+/**
+ * Functions prototype.
+ */
+inline void initializeMotors();
+inline void initializeControllers();
+void Move_r(float speed1);
+void Move_l(float speed2);
+inline void mesure_move_r_velocity();
+inline void mesure_move_l_velocity();
+inline void mesure_swing_velocity();
+inline void data_clear();
+inline void data_check();
+inline void SBDBT_interrupt();
+inline void initializeSBDBT();
+inline void calculate_diff();
+inline void Move_r_speed_following();
+inline void Move_l_speed_following();
+inline void Swing_speed_following();
+void Emergency_toggle();
+short toggle=0,edge=0;
+
+/**
+ * include function header files.
+ */
+#include "communicate.h"
+ 
+/**
+ * Defines
+ */
+#define RATE 0.01
+
+/**
+ * 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);
+//diff
+PID diff_controller(DIFF_Kc, DIFF_TAUi, DIFF_TAUd, RATE);
+//swing
+PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
+
+/***Ticker***/
+//Ticker interrupt;
+
+/***Variables***/
+double Pulses_move_r=0.0;
+double Pulses_move_l=0.0;
+double Pulses_swing=0.0;
+double PrefPulses_move_r=0.0;
+double PrefPulses_move_l=0.0;
+double PrefPulses_swing=0.0;
+double move_r_velocity=0.0;
+double move_l_velocity=0.0;
+double swing_velocity=0.0;
+//double Prefmove_r_velocity=0.0;
+//double Prefmove_l_velocity=0.0;
+double targ_move_r_velocity=0.0;
+double targ_move_l_velocity=0.0;
+double targ_swing_velocity=0.0;
+double diff=0.0;
+//const double alpha=0.9;
+
+/**
+ * Functions.
+ */
+
+/***The function is motors initialize.***/
+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(){    
+
+    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);
+    diff_controller.setInputLimits(DIFF_INPUT_LIMIT_BOTTOM, DIFF_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);
+    diff_controller.setBias(DIFF_BIAS);
+    swing_controller.setBias(SWING_BIAS);
+    
+    //set mode.
+    move_r_controller.setMode(AUTO_MODE);
+    move_l_controller.setMode(AUTO_MODE);
+    diff_controller.setMode(AUTO_MODE);
+    swing_controller.setMode(AUTO_MODE);
+    
+    //set point.
+    diff_controller.setSetPoint(0.0);
+}
+
+/***The function is Move on field.***/ 
+//Right
+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{
+        Move_r_Motor_CCW = 0;
+        Move_r_Motor_CW = 1;
+        Move_r_Motor_PWM = speed1;
+    }
+}
+//Left
+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{
+        Move_l_Motor_CCW = 0;
+        Move_l_Motor_CW = 1;
+        Move_l_Motor_PWM = speed2;
+    }
+}
+
+/***Caluculate move 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(){
+    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(){
+    Pulses_swing = (double)Swing_speed_sens.getPulses();
+    swing_velocity = Pulses_swing - PrefPulses_swing;
+    PrefPulses_swing = Pulses_swing;
+}
+
+/***The function is following move speed.***/
+float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
+short r=0,l=0;
+
+inline void calculate_diff(){
+    diff=move_r_velocity-move_l_velocity;
+    diff_controller.setProcessValue(diff);
+    if(diff<0) r=1,l=0;
+    else r=0,l=1;
+}
+
+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();
+    //if(r) cont_move_r=move_r_controller.compute()+diff_controller.compute();
+    //else cont_move_r=move_r_controller.compute();
+    Move_r(cont_move_r);
+}
+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();
+    //if(l) cont_move_l=move_l_controller.compute()+diff_controller.compute();
+    //else cont_move_l=move_l_controller.compute();
+    Move_l(cont_move_l);
+}
+inline void Swing_speed_following(){
+    swing_controller.setSetPoint((float)targ_swing_velocity);
+    swing_controller.setProcessValue((float)swing_velocity);
+    cont_swing = swing_controller.compute();
+    Motor_swing_pwm = cont_swing;
+}
+
+/***Emergency stop.***/
+void Emergency_toggle(){
+    if(edge){
+        edge=0;
+        if(toggle) toggle=0,Emergency_stop=0;
+        else toggle=1,Emergency_stop=1;
+    }
+}
+
+#endif /*machine.h*/