TurtleBot
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of
TurtleBot_v01
by 
worasuchad haomachai
Diff: main.cpp
- Revision:
- 6:8ae55e1f7e76
- Parent:
- 5:08334c6a42ca
- Child:
- 7:ca887fdc5388
--- a/main.cpp Wed Jul 11 05:38:01 2018 +0000
+++ b/main.cpp Fri Jul 20 17:44:07 2018 +0000
@@ -1,8 +1,8 @@
//////////////////////////////////////////////////////////////////
// project: TurtleBot Project //
-// code v.: 0.0 //
+// code v.: 0.3 //
// board : NUCLEO-F303KB //
-// date : 19/6/2018 //
+// date : 19/7/2018 //
// code by: Worasuchad Haomachai //
// detail : //
//////////////////////////////////////////////////////////////////
@@ -15,17 +15,16 @@
#include "attitude.h"
#include "math.h"
-Serial pc(USBTX, USBRX); //Serial Port
+Serial pc(USBTX, USBRX); // Serial Port
Timer timer1;
-Timer timerwalk;
Thread thread1;
Thread thread2;
-Servo Servo1(D6);
-Servo Servo2(D8);
-Servo Servo3(D9);
-Servo Servo4(D10);
+Servo Servo1(D4); // Servo Left Up (L1)
+Servo Servo2(D5); // Servo Left Down (L2)
+Servo Servo3(D8); // Servo Right Up (R1)
+Servo Servo4(D9); // Servo Right Down (R2)
///////////////////////// prototype func ///////////////////////
//////////////////////////////////////////////////////////////////
@@ -38,28 +37,29 @@
void servo_Right();
void receive_hormone();
-float calculateSD(float data[]);
-float calculateMean(float meanData[]);
+float calculateSD(float [], int );
+float calcSDGait(float [], int );
+float calculateMean(float [], int );
float HormoneCon(float );
-bool checkIMUFirst(float , float , float );
+bool checkIMUFirst(float , float, float );
////// debug func //////
void printStateGait();
///////////////////////// variable ///////////////////////////////
//////////////////////////////////////////////////////////////////
-// home param
-int walking_time;
+// global variable
int initCheck = 0;
float waittime = 0.001 ;
float round = 6;
+float ArrRoll[10], ArrPitch[10], ArrYaw[10];
-// hormone param
+// hormone variable
float Cg_down = 0.00;
float Cg_up = 0;
float Cg = 0.00, CgPrevious = 0.00;
-// servo motor param
+// servo motor variable
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
float down_degree = 90.00;
@@ -71,47 +71,61 @@
float pos_up_left = 1000.00;
float pos_down_end_left;
float pos_up_end_left;
-float state_count_left = 1;
-float round_count_left = 1;
float step_down_left;
float step_up_left;
+int state_count_left = 0;
+int round_count_left = 0;
+
// servo right side
float pos_down_right = 1400.00;
float pos_up_right = 1000.00;
float pos_down_end_right;
float pos_up_end_right;
-float state_count_right = 1;
-float round_count_right = 1;
float step_up_right;
float step_down_right;
+int state_count_right = 0;
+int round_count_right = 0;
-////// debug param //////
+// other variable
+uint32_t getIMUTimer;
+
+////// debug variable //////
// print state gait
int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
+int showIMUData = 0;
+
///////////////////////// main ////////////////////////////
//////////////////////////////////////////////////////////////////
int main()
{
- pc.baud(115200);
- timer1.start(); // start timer counting
- attitude_setup(); // IMU setup
- //thread1.start(IMU); // IMU thread start
- pc.printf(" Please press! '1' to start..\n\r");
+ pc.baud(115200);
+ attitude_setup(); // IMU setup
+ //thread1.start(IMU); // IMU thread start
+ //thread2.start(servo); // servo thread start
+ pc.printf(" Please press! '1' to start..\n\r");
if (pc.getc() == '1')
{
- thread1.start(IMU); // IMU thread start
- //thread2.start(servo); // servo thread start
- }
+ thread1.start(IMU); // IMU thread start
+ thread2.start(servo); // servo thread start
+ }
}
///////////////////////// IMU /////////////////////////////
//////////////////////////////////////////////////////////////////
void IMU()
{
- int i;
- float ArrayOfRoll[10], ArrayOfPitch[10], ArrayOfYaw[10];
+ int iterIMU = 0, iterArr = 0;
+ bool IMUWasStable = false;
+ float ArrayOfRoll[10] = {0.000f}, ArrayOfPitch[10] = {0.000f}, ArrayOfYaw[10] = {0.000f};
float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
float DiffOfRP = 0.0f;
+ ////// new ////////
+ int iterAxG2 = 0;
+ float ArrayOfAx_G2[80] = {0}, SDOfAx_G2;
+ int state_count_left_old, state_count_right_old;
+
+ timer1.start(); // start timer counting
+ getIMUTimer = timer1.read_ms();
/* pc.printf("roll\t");
pc.printf("Si\t");
pc.printf("Cg\t");
@@ -119,98 +133,131 @@
pc.printf("up\n"); */
while(1)
{
- if (timer1.read_us() >= 500) // read time in 0.5 ms
- {
+ if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 ms
+ {
attitude_get();
-
- //pc.printf(" %f \t", ax*10 );
- //pc.printf(" %f \t", ay*10 );
- //pc.printf(" %f \t", az*10-10); //cm/s*s
-
//pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw);
//pc.printf("up\t");
//pc.printf("%f \t",up_degree);
//pc.printf("%f\t",Cg);
- //pc.printf("down\t");
+ //pc.printf("down\t");
//pc.printf("%f \t",down_degree);
//pc.printf("%f\n",Cg);
////////////////////////// Signal Pre-Process every 10 ms //////////////////////
////////////////////////////////////////////////////////////////////////////////
- if(i < 10)
+ if(iterIMU < 10)
{
- ArrayOfRoll[i] = roll;
- ArrayOfPitch[i] = pitch;
- ArrayOfYaw[i] = yaw;
- //pc.printf("i = %i ,ArrayOfRoll = %.2f, roll= %.2f\n\r",i, ArrayOfRoll[i], roll);
- i++;
+ ArrayOfRoll[iterIMU] = roll;
+ ArrayOfPitch[iterIMU] = pitch;
+ ArrayOfYaw[iterIMU] = yaw;
+ //pc.printf("%i\t %.3f\t %.3f\t %.3f\n\r",i, roll, pitch, yaw);
+ iterIMU++;
}
- else // every 5 ms
+ else // every 10 ms
{
////// print state of gait /////
- //printStateGait();
+ printStateGait();
- // the accleration
- //pc.printf("%.3f\t\t", gx*PI/180.0f);
- //pc.printf("%.3f\t\t", gy*PI/180.0f);
- //pc.printf("%.3f\t\t", gz*PI/180.0f);
+ //pc.printf("%.3f\t\t %.3f\t\t %.3f\t\t", roll, pitch, yaw );
+ pc.printf("%.3f\t\t", roll);
+ pc.printf("%.3f\t\t", pitch);
+ pc.printf("%.3f\t\t", yaw);
+
+ for(iterArr = 0; iterArr < 10 ; iterArr++)
+ {
+ ArrRoll[iterArr] = ArrayOfRoll[iterArr];
+ ArrPitch[iterArr] = ArrayOfPitch[iterArr];
+ ArrYaw[iterArr] = ArrayOfYaw[iterArr];
+ }
+
+ // the accleration value
+ pc.printf("%.3f\t\t", gx*PI/180.0f);
+ pc.printf("%.3f\t\t", gy*PI/180.0f);
+ pc.printf("%.3f\t\t", gz*PI/180.0f);
// the gyro value
- //pc.printf("%.3f\t\t", ax * 9.81f ); // convert g to m/s^2
- //pc.printf("%.3f\t\t", ay * 9.81f); // convert g to m/s^2
- //pc.printf("%.3f\n\r", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
+ pc.printf("%.3f\t\t", ax * 9.81f ); // convert g to m/s^2
+ pc.printf("%.3f\t\t", ay * 9.81f); // convert g to m/s^2
+ pc.printf("%.3f\n\r", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
+
+ //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
+ //HormoneCon(SDOfRoll);
+
+ if(!IMUWasStable)
+ {
+ //////////// roll //////////////
+ SDOfRoll = calculateSD(ArrRoll, 10);
+ //pc.printf("%.3f\t\t", SDOfRoll);
+ //pc.printf("%.3f\t\t", ArrayOfRoll[9]);
+
+ //////////// pitch ///////////////
+ SDOfPitch = calculateSD(ArrPitch, 10 );
+ //pc.printf("%.3f\t\t", SDOfPitch);
+ //pc.printf("%.3f\t\t", ArrayOfPitch[9]);
+
+ //////////// yaw ///////////////
+ SDOfYaw = calculateSD(ArrYaw, 10 );
+ //pc.printf("%.3f\n\r", SDOfYaw);
+ //pc.printf("%.3f\t\t", ArrayOfYaw[9]);
+ }
- // diff roll pitch
+ if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
+ {
+ FirstOfRoll = calculateMean(ArrRoll, 10);
+ FirstOfPitch = calculateMean(ArrPitch, 10);
+ FirstOfYaw = calculateMean(ArrYaw, 10);
+ pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
+
+ state_count_left = 1;
+ round_count_left = 1;
+ state_count_right = 1;
+ round_count_right = 1;
+ IMUWasStable = true;
+ pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
+ }
+
+ ///////// SD of Ax in G2 ///////
+/* if(state_count_left == 2 and state_count_right == 2 )
+ {
+ //pc.printf("%d\t %.3f\n\r", iterAxG2, ax * 9.81f ); // convert g to m/s^2
+ ArrayOfAx_G2[iterAxG2++] = ax * 9.81f;
+ state_count_left_old = state_count_left;
+ state_count_right_old = state_count_right;
+ }
+ else if(state_count_left_old == 2 and state_count_right_old == 2 and state_count_left == 3 and state_count_right == 3)
+ {
+ SDOfAx_G2 = calcSDGait(ArrayOfAx_G2, iterAxG2);
+ //pc.printf("SD of AxG2 %.3f\n\r", SDOfAx_G2);
+ iterAxG2 = 0;
+ memset(ArrayOfAx_G2, 0, sizeof(ArrayOfAx_G2));
+ state_count_left_old = 0;
+ state_count_right_old = 0;
+ }
+
+ ///// distance form origin //////
if(state_count_left == 4 and state_count_right == 4 )
{
DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) );
- pc.printf("%.3f\n\r", DiffOfRP);
+ //pc.printf("DiffOfRP %.3f\n\r", DiffOfRP);
DiffOfRP = 0.0f;
}
- //////////// roll //////////////
- SDOfRoll = calculateSD(ArrayOfRoll);
- //pc.printf("%.3f\t\t", SDOfRoll);
- //pc.printf("%.3f\t\t", roll);
-
- //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
- //HormoneCon(SDOfRoll);
-
- //////////// pitch ///////////////
- SDOfPitch = calculateSD(ArrayOfPitch);
- //pc.printf("%.3f\t\t", SDOfPitch);
- //pc.printf("%.3f\t\t", pitch);
-
- //////////// yaw ///////////////
- SDOfYaw = calculateSD(ArrayOfYaw);
- //pc.printf("%.3f\t\t", SDOfYaw);
- //pc.printf("%.3f\n\r", yaw);
-
- if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
- {
- FirstOfRoll = calculateMean(ArrayOfRoll);
- FirstOfPitch = calculateMean(ArrayOfPitch);
- FirstOfYaw = calculateMean(ArrayOfYaw);
- pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
- thread2.start(servo); // Servo Thread
- pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
- }
-
+*/
//pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
// func find diff beween directOfRobot and MeanOfYaw
//pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
- /*pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
+/* pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
pc.printf("down: \t");
pc.printf("%f \t",down_degree);
pc.printf("up: \t");
- pc.printf("%f \n\r",up_degree);*/
-
+ pc.printf("%f \n\r",up_degree);
+*/
// reset iteration
- i = 0;
+ iterIMU = 0;
}
-
- timer1.reset(); // reset timer
+ getIMUTimer = timer1.read_ms();
}
}
}
@@ -219,13 +266,17 @@
//////////////////////////////////////////////////////////////////
void servo()
{
- /*Servo1.Enable(1000,20000);
+ Servo1.Enable(1000,20000);
Servo2.Enable(1000,20000);
Servo3.Enable(1000,20000);
- Servo4.Enable(1000,20000);*/
+ Servo4.Enable(1000,20000);
- pc.printf("\n\r Servo Start Ja!!! \n\r");
- timerwalk.start(); // start timer counting
+ Servo1.SetPosition(pos_down_left);
+ Servo2.SetPosition(pos_up_left);
+ Servo3.SetPosition(pos_down_right);
+ Servo4.SetPosition(pos_up_right);
+
+ //pc.printf("\n\r Servo Start Ja!!! \n\r");
while(1)
{
@@ -235,43 +286,16 @@
servo_Left(); // control left lag
servo_Right(); // control right leg
- // fin for walking
- if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round)
+ // FIN for walking
+ if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
{
thread1.terminate();
- pc.printf("Finish! \t");
- walking_time = timerwalk.read_ms();
- pc.printf("Walking time = %d ms\n\r", walking_time);
- break;
+ pc.printf("FIN! \n\r");
+ break;
}
}
}
-///////////////////////// receive_hormone /////////////////
-//////////////////////////////////////////////////////////////////
-void receive_hormone()
-{
- //down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
- down_degree = 85.00;
- /*pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
- pc.printf("down: \t");
- pc.printf("%f \t",down_degree);*/
- //pc.printf("%f\t",Cg);
- //up_degree = 45.00f*(1.00f+(0.7f*Cg_up));
- up_degree = 45.00;
- /*pc.printf("up: \t");
- pc.printf("%f \n\r",up_degree);*/
- //pc.printf("%f\n",Cg);
- if(down_degree < 85)
- {
- down_degree = 85;
- if(up_degree > 75)
- {
- up_degree = 75;
- }
- }
-}
-
///////////////////////// cal_step_down ///////////////////
//////////////////////////////////////////////////////////////////
void cal_step_down()
@@ -336,32 +360,6 @@
pc.printf("%f\t\t",step_up_right);;
pc.printf("step_up_left: ");
pc.printf("%f\n\r",step_up_left);*/
-}
-
-///////////////////// Print State Gait //////////////////////
-//////////////////////////////////////////////////////////////////
-void printStateGait()
-{
- if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
- {
- pc.printf("\n\r State Gait 1 \n\r");
- stateGaitOne = 1; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 0;
- }
- else if(state_count_left == 2 and state_count_right == 2 and stateGaitTwo == 0)
- {
- pc.printf("\n\r State Gait 2 \n\r");
- stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
- }
- else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
- {
- pc.printf("\n\r State Gait 3 \n\r");
- stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
- }
- else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
- {
- pc.printf("\n\r State Gait 4 \n\r");
- stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
- }
}
///////////////////////// servo_Left //////////////////////
@@ -417,14 +415,14 @@
pos_up_left -= step_up_left;
if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
{
- state_count_left = 0;
+ state_count_left = 5;
}
/*pc.printf("LDD");
pc.printf("%f\n",pos_down_left);
pc.printf("LDP");
pc.printf("%f\n",pos_up_left);*/
}
- else if (state_count_left == 0 and round_count_left < round)
+ else if (state_count_left == 5 and round_count_left < round)
{
round_count_left++;
state_count_left = 1;
@@ -486,14 +484,14 @@
pos_up_right = pos_up_right - step_up_right;
if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
{
- state_count_right = 0;
+ state_count_right = 5;
}
/*pc.printf("RDD");
pc.printf("%f\n",pos_down_right);
pc.printf("RDP");
pc.printf("%f\n",pos_up_right);*/
}
- else if (state_count_right == 0 and round_count_right < round)
+ else if (state_count_right == 5 and round_count_right < round)
{
round_count_right = round_count_right+1;
state_count_right = 1;
@@ -502,6 +500,84 @@
}
}
+///////////////////////// calcSDGait ///////////////////////
+//////////////////////////////////////////////////////////////////
+float calcSDGait(float sdDataGait[], int iterGait)
+{
+ float sum = 0.0, mean, standardDeviation = 0.0;
+ int i;
+
+ //pc.printf("iterGait = %d \n\r", iterGait);
+ for(i = 5; i < (iterGait + 1) - 5 ; i++)
+ {
+ sum += sdDataGait[i];
+ //pc.printf("sum = %.2f \n\r",sum);
+ }
+ mean = sum/iterGait;
+ //pc.printf("sum = %.2f \n\r",sum);
+ //pc.printf("mean = %.2f \n\r",mean);
+
+ for(i = 5; i < (iterGait + 1) - 5; i++)
+ {
+ standardDeviation += pow(sdDataGait[i] - mean, 2);
+ //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
+ }
+ //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
+ return sqrt(standardDeviation / iterGait);
+}
+
+///////////////////////// calculateSD /////////////////////
+//////////////////////////////////////////////////////////////////
+float calculateSD(float sdData[], int size)
+{
+ float sum = 0.0, mean, standardDeviation = 0.0;
+ int i;
+
+ for(i = 0; i < size ; i++)
+ {
+ sum += sdData[i];
+ //pc.printf("sum = %.2f \n\r",sum);
+ }
+ mean = sum/size;
+ //pc.printf("mean = %.2f \n\r",mean);
+
+ for(i = 0; i < size; i++)
+ {
+ standardDeviation += pow(sdData[i] - mean, 2);
+ //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
+ }
+ return sqrt(standardDeviation / size);
+}
+
+///////////////////////// calculateMean ///////////////////
+//////////////////////////////////////////////////////////////////
+float calculateMean(float meanData[], int size)
+{
+ float sum = 0.0, mean;
+ int i;
+
+ for(i = 0; i < size ; ++i)
+ {
+ sum += meanData[i];
+ //pc.printf("sum = %.2f \n\r",sum);
+ }
+ mean = sum/size;
+ //pc.printf("mean = %.2f \n\r",mean);
+ return mean;
+}
+
+///////////////////////// check IMU first //////////////////
+//////////////////////////////////////////////////////////////////
+bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
+{
+ if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
+ {
+ initCheck = 1;
+ return true;
+ }
+ return false;
+}
+
///////////////// Hormone Concentration /////////////////////
//////////////////////////////////////////////////////////////////
float HormoneCon(float SiPreProcess)
@@ -528,76 +604,53 @@
return Cg;
}
-///////////////////////// calculateSD /////////////////////
+///////////////////////// receive_hormone /////////////////
//////////////////////////////////////////////////////////////////
-float calculateSD(float sdData[])
+void receive_hormone()
{
- float sum = 0.0, mean, standardDeviation = 0.0;
- int i;
-
- for(i = 0; i < 10 ; ++i)
+ //down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
+ down_degree = 85.00;
+ /*pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
+ pc.printf("down: \t");
+ pc.printf("%f \t",down_degree);*/
+ //pc.printf("%f\t",Cg);
+ //up_degree = 45.00f*(1.00f+(0.7f*Cg_up));
+ up_degree = 45.00;
+ /*pc.printf("up: \t");
+ pc.printf("%f \n\r",up_degree);*/
+ //pc.printf("%f\n",Cg);
+ if(down_degree < 85)
{
- sum += sdData[i];
- //pc.printf("sum = %.2f \n\r",sum);
- }
- mean = sum/10;
- //pc.printf("mean = %.2f \n\r",mean);
-
- for(i = 0; i < 10; ++i)
- {
- standardDeviation += pow(sdData[i] - mean, 2);
- //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
+ down_degree = 85;
+ if(up_degree > 75)
+ {
+ up_degree = 75;
+ }
}
- return sqrt(standardDeviation / 10);
}
-///////////////////////// calculateMean ///////////////////
+///////////////////// Print State Gait //////////////////////
//////////////////////////////////////////////////////////////////
-float calculateMean(float meanData[])
+void printStateGait()
{
- float sum = 0.0, mean;
- int i;
-
- for(i = 0; i < 10 ; ++i)
+ if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
{
- sum += meanData[i];
- //pc.printf("sum = %.2f \n\r",sum);
- }
- mean = sum/10;
- //pc.printf("mean = %.2f \n\r",mean);
- return mean;
-}
-
-///////////////////////// check IMU first //////////////////
-//////////////////////////////////////////////////////////////////
-bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
-{
- if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
+ pc.printf("\n\r State Gait 1 \n\r");
+ stateGaitOne = 1; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 0;
+ }
+ else if(state_count_left == 2 and state_count_right == 2 and stateGaitTwo == 0)
{
- servoFirstState();
- initCheck++;
- }
- else if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 1)
- {
- initCheck++;
- return 1;
+ pc.printf("\n\r State Gait 2 \n\r");
+ stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
}
- return 0;
-}
-
-///////////////////// servo First State /////////////////////
-//////////////////////////////////////////////////////////////////
-void servoFirstState()
-{
- Servo1.Enable(1000,20000);
- Servo2.Enable(1000,20000);
- Servo3.Enable(1000,20000);
- Servo4.Enable(1000,20000);
- pc.printf("Servo First State\n\r");
- pc.printf("pos_down_left: %.3f\t pos_up_left: %.3f\t pos_down_right: %.3f\t pos_up_right: %.3f\n\r", pos_down_left, pos_up_left, pos_down_right, pos_up_right);
- Servo1.SetPosition(pos_down_left);
- Servo2.SetPosition(pos_up_left);
- Servo3.SetPosition(pos_down_right);
- Servo4.SetPosition(pos_up_right);
-
+ else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
+ {
+ pc.printf("\n\r State Gait 3 \n\r");
+ stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
+ }
+ else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
+ {
+ pc.printf("\n\r State Gait 4 \n\r");
+ stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
+ }
}
\ No newline at end of file