Diff: machine_ps3.h

Revision:

0:b613dc16f27d

Child:

1:3ac2087996f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/machine_ps3.h	Wed Oct 28 09:03:19 2015 +0000
@@ -0,0 +1,116 @@
+/**
+ * This library is included main.cpp.
+ * class "machine" whitch machine functions are structured.
+ */
+
+#ifndef MACHINE_H
+#define MACHINE_H
+
+/**
+ * Functions prototype.
+ */
+
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include <stdlib.h>
+#include <string.h>
+#include "QEI.h"
+#include "PID.h"
+
+#ifdef BLUE
+#include "PinDefinedSettingBlue.h"
+#else
+#include "PinDefinedSettingRed.h"
+#endif
+
+#include "prototype.h"
+#include "communicate.h"
+#include "Parameters_ps3.h"
+#include "Swing.h"
+#include "manualMode.h"
+#include "autoMode.h"
+
+/**
+ * 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);
+}
+
+/***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    = -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    = -speed2;
+    } else {
+        Move_l_Motor_CCW    = 0;
+        Move_l_Motor_CW     = 1;
+        Move_l_Motor_PWM    = speed2;
+    }
+}
+
+/***Caluculate state.***/
+inline void mesure_state()
+{
+    Pulses_move_r       = ( double )Move_r_sense.getPulses();
+    Pulses_move_l       = ( double )Move_l_sense.getPulses();
+    dlr                 = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
+    dll                 = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
+    PrefPulses_move_r   = Pulses_move_r;
+    PrefPulses_move_l   = Pulses_move_l;
+    vr                  = dlr / RATE;
+    vl                  = dll / RATE;
+    if(flagf) velocity  = ( vr + vl ) / 2.0;
+    else  velocity      = -( vr + vl ) / 2.0;
+    dsita               = ( dlr - dll ) / ( 2.0 * d );
+    dx                  = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
+    dy                  = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
+    sita                += dsita;
+    x                   += dx;
+    y                   += dy;
+}
+
+void resetState(){
+    wait(0.1);
+#ifdef BLUE
+    x=0.0;
+    y=0.0;
+    sita=PI/4.0;
+    targ_sita=PI/4.0;
+#else
+    x=0.0;
+    y=0.0;
+    sita=-PI/4.0;
+    targ_sita=-PI/4.0;
+#endif
+    targ_velocity=0.0;
+    Move_r_sense.reset();
+    Move_l_sense.reset();
+    velocity_controller.reset();
+    direction_controller.reset();
+}
+
+#endif /*machine.h*/