TurtleBot
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of
TurtleBot_v01
by 
worasuchad haomachai
Diff: main.cpp
- Revision:
- 8:865535fcf917
- Parent:
- 7:ca887fdc5388
- Child:
- 11:8548536c3f11
--- a/main.cpp Fri Jul 20 18:42:13 2018 +0000
+++ b/main.cpp Mon Aug 13 11:39:16 2018 +0000
@@ -1,22 +1,30 @@
-//////////////////////////////////////////////////////////////////
-// project: TurtleBot Project //
-// code v.: 0.3 //
-// board : NUCLEO-F303KB //
-// date : 19/7/2018 //
-// code by: Worasuchad Haomachai //
-// detail : //
-//////////////////////////////////////////////////////////////////
-///////////////////////// init ////////////////////////////////
-//////////////////////////////////////////////////////////////////
+/***************************< File comment >***************************/
+/* project: TurtleBot Project */
+/* project description : - */
+/*--------------------------------------------------------------------*/
+/* version : 0.8 */
+/* board : NUCLEO-F411RE */
+/* detail : two hg is added */
+/*--------------------------------------------------------------------*/
+/* create : 25/07/2018 */
+/* programmer : Worasuchad Haomachai */
+/**********************************************************************/
+
+/*--------------------------------------------------------------------*/
+/* Include file */
+/*--------------------------------------------------------------------*/
+#include <stdlib.h>
#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "attitude.h"
#include "math.h"
+#include "calculator.h"
+#include "hormone.h"
+#include "PowerMon.h"
Serial pc(USBTX, USBRX); // Serial Port
-
Timer timer1;
Thread thread1;
Thread thread2;
@@ -26,44 +34,36 @@
Servo Servo3(D8); // Servo Right Up (R1)
Servo Servo4(D9); // Servo Right Down (R2)
-///////////////////////// prototype func ///////////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* prototype fun */
+/*--------------------------------------------------------------------*/
void IMU();
void servo();
+void servoLeft();
+void servoRight();
void servoFirstState();
-void cal_step_down();
-void cal_step_up();
-void servo_Left();
-void servo_Right();
-
-void receive_hormone();
-float calculateSD(const float *, int );
-float calcSDGait(float [], int );
-float calculateMean(float [], int );
-float HormoneCon(float );
+void calcStepDown(float);
+void calcStepUp(float);
bool checkIMUFirst(float , float, float );
-////// debug func //////
+/* debug func */
void printStateGait();
-///////////////////////// variable ///////////////////////////////
-//////////////////////////////////////////////////////////////////
-// global variable
+/*--------------------------------------------------------------------*/
+/* Global variable */
+/*--------------------------------------------------------------------*/
+// home variable
int initCheck = 0;
float waittime = 0.001 ;
float round = 6;
-float ArrRoll[10], ArrPitch[10], ArrYaw[10];
-// hormone variable
-float Cg_down = 0.00;
-float Cg_up = 0;
-float Cg = 0.00, CgPrevious = 0.00;
+// interface wt hormone variable
+float upDeg = 45.00;
+float downDeg = 95.00;
// servo motor variable
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
-float down_degree = 90.00;
-float up_degree = 45.00;
float stepmin = 1.5;
// servo left side
@@ -88,172 +88,274 @@
// other variable
uint32_t getIMUTimer;
+uint32_t walkingTimer;
-////// debug variable //////
+/* debug variable */
// print state gait
int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
-int showIMUData = 0;
-///////////////////////// main ////////////////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* main */
+/*--------------------------------------------------------------------*/
int main()
{
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");
+ attitude_setup(); // IMU setup
+ pc.printf(" Please press! '1' to start..\n\r");
if (pc.getc() == '1')
{
+ timer1.start(); // start timer counting
thread1.start(IMU); // IMU thread start
thread2.start(servo); // servo thread start
}
}
-///////////////////////// IMU /////////////////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* IMU */
+/*--------------------------------------------------------------------*/
void IMU()
{
- int iterIMU = 0, iterArr = 0;
+ powerMon pmR2(-1);
+ powerMon pmR1(2000);
+ powerMon pmL2(560);
+ powerMon pmL1(3600);
+
+ calculator calc;
+ hormone hg;
+
+ int iterIMU = 0, state_count_left_old = 0, state_count_right_old = 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;
+ float *vAx, *vAy, *vAz;
+ float *pRoll, *pPitch;
+
+ /* param of State 2 */
+ int iterAG2 = 0;
+ float sdVectorAG2 = 0.0;
+
+ /* param of State 3 */
+ int iterAG3 = 0;
+ float sdVectorAG3 = 0.0;
+
+ /* param of State 4 */
+ int iterG4 = 0;
+ float meanG4 = 0.0;
- timer1.start(); // start timer counting
+ /* memory allocate for G2 and G3 */
+ vAx = (float *)malloc(50*sizeof(float));
+ vAy = (float *)malloc(50*sizeof(float));
+ vAz = (float *)malloc(50*sizeof(float));
+ /* check malloc is not return NULL */
+ if( vAx == NULL or vAy == NULL or vAz == NULL )
+ {
+ pc.printf("Error: memory could not be allocated in vectorA!\n\r");
+ }
+
+ /* memory allocate for G4 */
+ pRoll = (float *)malloc(50*sizeof(float));
+ pPitch = (float *)malloc(50*sizeof(float));
+ /* check malloc is not return NULL */
+ if( pRoll == NULL or pPitch == NULL )
+ {
+ pc.printf("Error: memory could not be allocated in pointG4!\n\r");
+ }
+
getIMUTimer = timer1.read_ms();
-/* pc.printf("roll\t");
- pc.printf("Si\t");
- pc.printf("Cg\t");
- pc.printf("down\t");
- pc.printf("up\n"); */
while(1)
{
- if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 ms
+ if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 ms
{
attitude_get();
- //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("%f \t",down_degree);
- //pc.printf("%f\n",Cg);
-
- ////////////////////////// Signal Pre-Process every 10 ms //////////////////////
- ////////////////////////////////////////////////////////////////////////////////
+ /*--------------------------------------------------------------------*/
+ /* Signal Pre-Process every 10 ms */
+ /*--------------------------------------------------------------------*/
if(iterIMU < 10)
{
- ArrayOfRoll[iterIMU] = roll;
- ArrayOfPitch[iterIMU] = pitch;
- ArrayOfYaw[iterIMU] = yaw;
- //pc.printf("%i\t %.3f\t %.3f\t %.3f\n\r",i, roll, pitch, yaw);
+ if(!IMUWasStable)
+ {
+ 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 10 ms
{
- ////// print state of gait /////
- printStateGait();
+ /* print state of gait */
+ //printStateGait();
+
+ /* roll pitch yaw */
+ //pc.printf("%.3f\t\t %.3f\t\t %.3f\n\r", roll, pitch, yaw );
+ //pc.printf("%.3f\t", roll);
+ //pc.printf("%.3f\t", pitch);
+ //pc.printf("%.3f\t\t", yaw);
+
+ /* the accleration value */
+ //pc.printf("%.3f\t", gx*PI/180.0f);
+ //pc.printf("%.3f\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 gyro value */
+ //pc.printf("%.3f\t", ax * 9.81f ); // convert g to m/s^2
+ //pc.printf("%.3f\t", ay * 9.81f); // convert g to m/s^2
+ //pc.printf("%.3f\t\t", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
- // 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("HC: %.3f\n\r", HormoneCon(SDOfRoll));
- //HormoneCon(SDOfRoll);
-
+ /* power monitoring */
+ //pc.printf("%.3f\t", pmL1.Power());
+ //pc.printf("%.3f\t", pmR1.Power());
+ //pc.printf("%.3f\t", pmL2.Power());
+ //pc.printf("%.3f\t\t", pmR2.Power());
+ /*--------------------------------------------------------------------*/
+ /* IMU check whetherbe stable or not before servo begin */
+ /*--------------------------------------------------------------------*/
if(!IMUWasStable)
{
//////////// roll //////////////
- SDOfRoll = calculateSD(ArrRoll, 10);
- //pc.printf("%.3f\t\t", SDOfRoll);
+ SDOfRoll = calc.calculateSD(ArrayOfRoll, 10);
+ //SDOfRoll = calculateSD(ArrRoll, 10);
+ //pc.printf("%.3f\t", SDOfRoll);
//pc.printf("%.3f\t\t", ArrayOfRoll[9]);
//////////// pitch ///////////////
- SDOfPitch = calculateSD(ArrPitch, 10 );
- //pc.printf("%.3f\t\t", SDOfPitch);
+ SDOfPitch = calc.calculateSD(ArrayOfPitch, 10);
+ //SDOfPitch = calculateSD(ArrPitch, 10 );
+ //pc.printf("%.3f\t", SDOfPitch);
//pc.printf("%.3f\t\t", ArrayOfPitch[9]);
//////////// yaw ///////////////
- SDOfYaw = calculateSD(ArrYaw, 10 );
+ SDOfYaw = calc.calculateSD(ArrayOfYaw, 10);
+ //SDOfYaw = calculateSD(ArrYaw, 10 );
//pc.printf("%.3f\n\r", SDOfYaw);
//pc.printf("%.3f\t\t", ArrayOfYaw[9]);
}
if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
{
- FirstOfRoll = calculateMean(ArrRoll, 10);
- FirstOfPitch = calculateMean(ArrPitch, 10);
- FirstOfYaw = calculateMean(ArrYaw, 10);
+ FirstOfRoll = calc.calculateMean(ArrayOfRoll, 10);
+ FirstOfPitch = calc.calculateMean(ArrayOfPitch, 10);
+ FirstOfYaw = calc.calculateMean(ArrayOfYaw, 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;
+ walkingTimer = timer1.read_ms();
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 )
+ }
+ /*--------------------------------------------------------------------*/
+ /* power monitoring in J */
+ /*--------------------------------------------------------------------*/
+
+ /*--------------------------------------------------------------------*/
+ /* || A || in State 2 */
+ /* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
+ /*--------------------------------------------------------------------*/
+
+ if(state_count_left == 2 and state_count_right == 2)
+ {
+ vAx[iterAG2] = ax*9.81f;
+ vAy[iterAG2] = ay*9.81f;
+ vAz[iterAG2] = ( az - 1 ) * 9.81f;
+ iterAG2++;
+ 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)
{
- //pc.printf("%d\t %.3f\n\r", iterAxG2, ax * 9.81f ); // convert g to m/s^2
- ArrayOfAx_G2[iterAxG2++] = ax * 9.81f;
+ // calculate SD of size vector A in G2 //
+ sdVectorAG2 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG2);
+ //pc.printf("%.3f\t", sdVectorAG2);
+
+ // hormone concentration //
+ //hg.hormoneCon(sdVectorAG2);
+ //pc.printf("HG2 %.3f\n\r", hg.Cg);
+
+ iterAG2 = 0;
+ state_count_left_old = 0;
+ state_count_right_old = 0;
+ //free(vAz);free(vAy);free(vAz);
+ }
+
+ /*--------------------------------------------------------------------*/
+ /* || A || in State 3 */
+ /* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
+ /*--------------------------------------------------------------------*/
+ else if(state_count_left == 3 and state_count_right == 3)
+ {
+ vAx[iterAG3] = ax*9.81f;
+ vAy[iterAG3] = ay*9.81f;
+ vAz[iterAG3] = ( az - 1 ) * 9.81f;
+ iterAG3++;
+ state_count_left_old = state_count_left;
+ state_count_right_old = state_count_right;
+ }
+ else if(state_count_left_old == 3 and state_count_right_old == 3 and state_count_left == 4 and state_count_right == 4)
+ {
+ // calculate SD of size vector A in G3 //
+ sdVectorAG3 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG3);
+ //pc.printf("%.3f\t", sdVectorAG3);
+
+ // hormone concentration //
+ //hg.hormoneCon(sdVectorAG3);
+ //pc.printf("HG3 %.3f\n\r", hg.Cg);
+
+ iterAG3 = 0;
+ state_count_left_old = 0;
+ state_count_right_old = 0;
+ //free(vAz);free(vAy);free(vAz);
+ }
+
+ /*--------------------------------------------------------------------*/
+ /* (rall,pitch) in State 4 */
+ /* distance form origin (0,0) */
+ /*--------------------------------------------------------------------*/
+
+ else if(state_count_left == 4 and state_count_right == 4 )
+ {
+ pRoll[iterG4] = roll;
+ pPitch[iterG4] = pitch;
+ iterG4++;
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)
+ else if(state_count_left_old == 4 and state_count_right_old == 4 and state_count_left == 1 and state_count_right == 1)
{
- 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));
+ // calculate SD of size vector A in G3 //
+ meanG4 = calc.calcG4(pRoll, pPitch,iterG4);
+ //pc.printf("%.3f\t", meanG4);
+
+ // hormone concentration //
+ upDeg = hg.upHG(sdVectorAG2, meanG4);
+ downDeg = hg.downHG(sdVectorAG2, sdVectorAG3);
+ pc.printf("%.3f\t", hg.cgDown);
+ pc.printf("%.3f\t", hg.cgUp);
+ pc.printf("%.3f\t", hg.hormoneRecDown(downDeg));
+ pc.printf("%.3f\n\r", hg.hormoneRecUp(upDeg));
+
+ iterG4 = 0;
state_count_left_old = 0;
state_count_right_old = 0;
+ //free(pRoll);free(pPitch);
}
- ///// distance form origin //////
- if(state_count_left == 4 and state_count_right == 4 )
+ /*--------------------------------------------------------------------*/
+ /* FIN for walking */
+ /*--------------------------------------------------------------------*/
+
+ if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
{
-
- DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) );
- //pc.printf("DiffOfRP %.3f\n\r", DiffOfRP);
- DiffOfRP = 0.0f;
+ pc.printf("TIME %d \n\r", timer1.read_ms() - walkingTimer);
+ pc.printf("FIN! \n\r");
+ //thread1.terminate();
+ free(vAz);free(vAy);free(vAz);
+ free(pRoll);free(pPitch);
+ break;
}
-*/
- //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
iterIMU = 0;
}
@@ -262,10 +364,13 @@
}
}
-///////////////////////// servo ///////////////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* servo */
+/*--------------------------------------------------------------------*/
void servo()
-{
+{
+ hormone hr;
+
Servo1.Enable(1000,20000);
Servo2.Enable(1000,20000);
Servo3.Enable(1000,20000);
@@ -275,35 +380,32 @@
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)
{
- receive_hormone();
- cal_step_down(); // return "step_down_right" and "step_down_left"
- cal_step_up(); // return "step_up_right" and "step_up_left"
- servo_Left(); // control left lag
- servo_Right(); // control right leg
+ calcStepUp( hr.hormoneRecUp(upDeg) ); // return "step_up_right" and "step_up_left"
+ calcStepDown( hr.hormoneRecDown(downDeg) ); // return "step_down_right" and "step_down_left"
+ servoLeft(); // control left lag
+ servoRight(); // control right leg
// 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("FIN! \n\r");
+ //pc.printf("TIME %d \n\r", timer1.read_ms() - walkingTimer);
+ //pc.printf("FIN! \n\r");
+ //thread2.terminate();
break;
}
}
}
-///////////////////////// cal_step_down ///////////////////
-//////////////////////////////////////////////////////////////////
-void cal_step_down()
+/*--------------------------------------------------------------------*/
+/* calculate step of servo down */
+/*--------------------------------------------------------------------*/
+void calcStepDown(float hormDown)
{
- //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 = (1060.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700
+ pos_down_end_left = (1000.00 + ((700.00/90.00)*( hormDown ))); // get degree for hormone receiver about downDegree ~ 90*,
+ pos_down_end_right = (1060.00 + ((700.00/90.00)*( hormDown ))); // 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
@@ -319,24 +421,15 @@
step_down_right = stepmin;
step_down_left = stepmin;
}
- /*pc.printf("pos_down_right: ");
- pc.printf("%f\t\t",pos_down_end_right);
- pc.printf("pos_down_left: ");
- pc.printf("%f\t\t",pos_down_end_left);
- pc.printf("step_down_right: ");
- pc.printf("%f\t\t",step_down_right);
- pc.printf("step_down_left: ");
- pc.printf("%f\n\r",step_down_left);*/
}
-///////////////////////// cal_step_up /////////////////////
-//////////////////////////////////////////////////////////////////
-void cal_step_up()
-{
- //pc.printf("up");
- //pc.printf("%f \n",up_degree);
- pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*,
- pos_up_end_right = 1000.00 + ((700.00/90.00)*(up_degree)); // so both pos_up_end_left and pos_up_end_right are around 1350
+/*--------------------------------------------------------------------*/
+/* calculate step of servo up */
+/*--------------------------------------------------------------------*/
+void calcStepUp(float hormUp)
+{
+ pos_up_end_left = 1000.00 + ((700.00/90.00)*( hormUp )); // get degree for hormone receiver about upDegree ~ 45*,
+ pos_up_end_right = 1000.00 + ((700.00/90.00)*( hormUp )); // so both pos_up_end_left and pos_up_end_right are around 1350
if (pos_up_end_right > pos_up_end_left)
{
step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); //stepmin = 1, pos_up_start = 1000.00
@@ -352,19 +445,12 @@
step_up_right = stepmin;
step_up_left = stepmin;
}
- /*pc.printf("pos_up_right: ");
- pc.printf("%f\t\t",pos_up_end_right);
- pc.printf("pos_up_left: ");
- pc.printf("%f\t\t",pos_up_end_left);
- pc.printf("step_up_right: ");
- pc.printf("%f\t\t",step_up_right);;
- pc.printf("step_up_left: ");
- pc.printf("%f\n\r",step_up_left);*/
}
-///////////////////////// servo_Left //////////////////////
-//////////////////////////////////////////////////////////////////
-void servo_Left()
+/*--------------------------------------------------------------------*/
+/* servo in left side */
+/*--------------------------------------------------------------------*/
+void servoLeft()
{
if(state_count_left == 1)
{
@@ -375,10 +461,6 @@
{
state_count_left = 2;
}
- /*pc.printf("LAD");
- pc.printf("%f\n",pos_down_left);
- pc.printf("LAP");
- pc.printf("%f\n",pos_up_left);*/
}
else if(state_count_left == 2)
{
@@ -389,10 +471,6 @@
{
state_count_left = 3;
}
- /*pc.printf("LBD");
- pc.printf("%f\n",pos_down_left);
- pc.printf("LBP");
- pc.printf("%f\n",pos_up_left);*/
}
else if(state_count_left == 3)
{
@@ -403,10 +481,6 @@
{
state_count_left = 4;
}
- /*pc.printf("LCD");
- pc.printf("%f\n",pos_down_left);
- pc.printf("LCP");
- pc.printf("%f\n",pos_up_left);*/
}
else if(state_count_left == 4)
{
@@ -417,10 +491,6 @@
{
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 == 5 and round_count_left < round)
{
@@ -431,9 +501,10 @@
}
}
-///////////////////////// servo_Right /////////////////////
-//////////////////////////////////////////////////////////////////
-void servo_Right()
+/*--------------------------------------------------------------------*/
+/* servo in right side */
+/*--------------------------------------------------------------------*/
+void servoRight()
{
if(state_count_right == 1)
{
@@ -444,10 +515,6 @@
{
state_count_right = 2;
}
- /*pc.printf("RAD");
- pc.printf("%f\n",pos_down_right);
- pc.printf("RAP");
- pc.printf("%f\n",pos_up_right);*/
}
else if(state_count_right == 2)
{
@@ -458,10 +525,6 @@
{
state_count_right = 3;
}
- /*pc.printf("RBD");
- pc.printf("%f\n",pos_down_right);
- pc.printf("RBP");
- pc.printf("%f\n",pos_up_right);*/
}
else if(state_count_right == 3)
{
@@ -472,10 +535,6 @@
{
state_count_right = 4;
}
- /*pc.printf("RCD");
- pc.printf("%f\n",pos_down_right);
- pc.printf("RCP");
- pc.printf("%f\n",pos_up_right);*/
}
else if(state_count_right == 4)
{
@@ -486,10 +545,6 @@
{
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 == 5 and round_count_right < round)
{
@@ -500,79 +555,9 @@
}
}
-///////////////////////// 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(const float *sdData, int size)
-{
- float sum = 0.0, mean, standardDeviation = 0.0;
- int i;
-
- if(!sdData)
- {
- return;
- }
-
- 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 //////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* 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)
@@ -583,79 +568,34 @@
return false;
}
-///////////////// Hormone Concentration /////////////////////
-//////////////////////////////////////////////////////////////////
-float HormoneCon(float SiPreProcess)
-{
- float CgTemp;
-
- //pc.printf("SiPreProcess: %.3f\t", SiPreProcess);
- //pc.printf("CgPrevious: %.3f\t", CgPrevious);
-
- ////// hormone gland //////
- CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
- //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));
- //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious));
- CgPrevious = Cg;
- //*********//
- Cg_down = Cg;
- Cg_up = Cg;
-
- return Cg;
-}
-
-///////////////////////// 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;
- }
- }
-}
-
-///////////////////// Print State Gait //////////////////////
-//////////////////////////////////////////////////////////////////
+/*--------------------------------------------------------------------*/
+/* print state gait for debuging */
+/*--------------------------------------------------------------------*/
void printStateGait()
{
if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
{
- pc.printf("\n\r State Gait 1 \n\r");
+ //pc.printf("\n\r State Gait 1 \n\r");
+ pc.printf("\n\rG1\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \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");
+ //pc.printf("\n\r State Gait 2 \n\r");
+ pc.printf("\n\rG2\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \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");
+ //pc.printf("\n\r State Gait 3 \n\r");
+ pc.printf("\n\rG3\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \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");
+ //pc.printf("\n\r State Gait 4 \n\r");
+ pc.printf("\n\rG4\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
}
-}
\ No newline at end of file
+}
+/*--------------------------------------------------------------------*/
\ No newline at end of file