turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 8:865535fcf917
- Parent:
- 7:ca887fdc5388
- Child:
- 11:8548536c3f11
diff -r ca887fdc5388 -r 865535fcf917 main.cpp --- 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