TurtleBot
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of
TurtleBot_v01
by 
worasuchad haomachai
Diff: main.cpp
- Revision:
- 5:08334c6a42ca
- Parent:
- 4:ec7e68b84f2b
- Child:
- 6:8ae55e1f7e76
--- a/main.cpp Thu Jun 21 08:12:44 2018 +0000
+++ b/main.cpp Wed Jul 11 05:38:01 2018 +0000
@@ -4,7 +4,7 @@
// board : NUCLEO-F303KB //
// date : 19/6/2018 //
// code by: Worasuchad Haomachai //
-// detail : Modify IMU thread //
+// detail : //
//////////////////////////////////////////////////////////////////
///////////////////////// init ////////////////////////////////
@@ -31,24 +31,28 @@
//////////////////////////////////////////////////////////////////
void IMU();
void servo();
-/////// In servo //////
+void servoFirstState();
void cal_step_down();
void cal_step_up();
void servo_Left();
void servo_Right();
-//////////////////////
+
void receive_hormone();
-
float calculateSD(float data[]);
float calculateMean(float meanData[]);
-float HormoneCon(float SiPreProcess);
-bool directionRobot(float directionMean, float directionSD);
+float HormoneCon(float );
+bool checkIMUFirst(float , float , float );
+
+////// debug func //////
+void printStateGait();
///////////////////////// variable ///////////////////////////////
//////////////////////////////////////////////////////////////////
// home param
int walking_time;
-int initDirection = 0;
+int initCheck = 0;
+float waittime = 0.001 ;
+float round = 6;
// hormone param
float Cg_down = 0.00;
@@ -60,10 +64,9 @@
float pos_up_start = 1000.00;
float down_degree = 90.00;
float up_degree = 45.00;
-float stepmin = 1;
-float round = 5;
-float waittime = 0.001 ;
+float stepmin = 1.5;
+// servo left side
float pos_down_left = 1400.00;
float pos_up_left = 1000.00;
float pos_down_end_left;
@@ -72,7 +75,7 @@
float round_count_left = 1;
float step_down_left;
float step_up_left;
-
+// servo right side
float pos_down_right = 1400.00;
float pos_up_right = 1000.00;
float pos_down_end_right;
@@ -82,6 +85,9 @@
float step_up_right;
float step_down_right;
+////// debug param //////
+// print state gait
+int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
///////////////////////// main ////////////////////////////
//////////////////////////////////////////////////////////////////
int main()
@@ -103,8 +109,9 @@
void IMU()
{
int i;
- float ArrayOfRoll[10], ArrayOfYaw[10];
- float SDOfRoll, SDOfYaw, MeanOfYaw, directOfRobot = 0.00;
+ float ArrayOfRoll[10], ArrayOfPitch[10], ArrayOfYaw[10];
+ float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
+ float DiffOfRP = 0.0f;
/* pc.printf("roll\t");
pc.printf("Si\t");
pc.printf("Cg\t");
@@ -112,7 +119,7 @@
pc.printf("up\n"); */
while(1)
{
- if (timer1.read_us() >= 1000) // read time in 1 ms
+ if (timer1.read_us() >= 500) // read time in 0.5 ms
{
attitude_get();
@@ -121,7 +128,6 @@
//pc.printf(" %f \t", az*10-10); //cm/s*s
//pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw);
- pc.printf("%f\n\r",yaw);
//pc.printf("up\t");
//pc.printf("%f \t",up_degree);
//pc.printf("%f\t",Cg);
@@ -134,44 +140,72 @@
if(i < 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++;
}
- else
+ else // every 5 ms
{
+ ////// print state of gait /////
+ //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);
+
+ // 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
+
+ // diff roll pitch
+ if(state_count_left == 4 and state_count_right == 4 )
+ {
+
+ DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) );
+ pc.printf("%.3f\n\r", DiffOfRP);
+ DiffOfRP = 0.0f;
+ }
//////////// roll //////////////
- //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(ArrayOfRoll)); // every 10 ms
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);
+ //HormoneCon(SDOfRoll);
+
+ //////////// pitch ///////////////
+ SDOfPitch = calculateSD(ArrayOfPitch);
+ //pc.printf("%.3f\t\t", SDOfPitch);
+ //pc.printf("%.3f\t\t", pitch);
//////////// yaw ///////////////
- MeanOfYaw = calculateMean(ArrayOfYaw);
- pc.printf("MeanOfYaw: %.3f\n\r", MeanOfYaw);
SDOfYaw = calculateSD(ArrayOfYaw);
- pc.printf("SDOfYaw: %.3f\n\r", SDOfYaw);
+ //pc.printf("%.3f\t\t", SDOfYaw);
+ //pc.printf("%.3f\n\r", yaw);
- if(directionRobot(MeanOfYaw, SDOfYaw)) // only one time for comming
+ if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
{
- directOfRobot = MeanOfYaw; // direction of robot
+ 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("::::::::::::::::::::::::::: Init Direction OK :::::::::::::::::::::::::::\n\r");
+ pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
}
- pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
-
- if(directOfRobot > SDOfYaw)
- {
- pc.printf("Turn Left: %.3f\n\r", (directOfRobot - MeanOfYaw));
- }
- else
- {
- pc.printf("Turn Right: %.3f\n\r", (MeanOfYaw - directOfRobot));
- }
-
+
+ //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("down: \t");
+ pc.printf("%f \t",down_degree);
+ pc.printf("up: \t");
+ pc.printf("%f \n\r",up_degree);*/
+
// reset iteration
i = 0;
}
@@ -185,21 +219,14 @@
//////////////////////////////////////////////////////////////////
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("Servo Start Ja!!!\n");
+ pc.printf("\n\r Servo Start Ja!!! \n\r");
timerwalk.start(); // start timer counting
- /*pc.printf("Si\t");
- pc.printf("%f \t",Si);
- pc.printf("down\t");
- pc.printf("%f \t",down_degree);
- pc.printf("%f\t",Cg);
- pc.printf("up\t");
- pc.printf("%f \t",up_degree);
- pc.printf("%f\n",Cg);*/
+
while(1)
{
receive_hormone();
@@ -225,15 +252,16 @@
void receive_hormone()
{
//down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
- down_degree = 83.50;
- /*pc.printf("down\t");
- pc.printf("%f \t",down_degree);
- pc.printf("%f\t",Cg);*/
+ 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 \t",up_degree);
- pc.printf("%f\n",Cg);
+ /*pc.printf("up: \t");
+ pc.printf("%f \n\r",up_degree);*/
+ //pc.printf("%f\n",Cg);
if(down_degree < 85)
{
down_degree = 85;
@@ -241,7 +269,7 @@
{
up_degree = 75;
}
- }*/
+ }
}
///////////////////////// cal_step_down ///////////////////
@@ -251,7 +279,7 @@
//pc.printf("down");
//pc.printf("%f \n",down_degree);
pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*,
- pos_down_end_right = (1070.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700
+ pos_down_end_right = (1060.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700
if (pos_down_end_right > pos_down_end_left)
{
step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); //stepmin = 1, pos_down_start = 1400.00
@@ -310,6 +338,32 @@
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 //////////////////////
//////////////////////////////////////////////////////////////////
void servo_Left()
@@ -459,8 +513,9 @@
////// hormone gland //////
CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
- //pc.printf("CgTemp: %.3f\t", CgTemp);
+ //pc.printf("CgTemp: %.3f\t\t", CgTemp);
Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1
+ //pc.printf("Cg: %.3f\n\r", Cg);
///////////////////////////
//pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess));
@@ -513,14 +568,36 @@
return mean;
}
-///////////////////////// directionRobot //////////////////
+///////////////////////// check IMU first //////////////////
//////////////////////////////////////////////////////////////////
-bool directionRobot(float directionMean, float directionSD)
+bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
{
- if( directionSD < 0.06f and initDirection == 0 )
+ if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
+ {
+ servoFirstState();
+ initCheck++;
+ }
+ else if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 1)
{
- initDirection++;
- return 1;
- }
+ initCheck++;
+ return 1;
+ }
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);
+
}
\ No newline at end of file