Update hormone config
Dependencies: mbed Servo PM mbed-rtos hormone calculator
main.cpp
- Committer:
- worasuchad
- Date:
- 2018-08-24
- Revision:
- 12:a9d894e37936
- Parent:
- 11:8548536c3f11
- Child:
- 14:d084ea982238
File content as of revision 12:a9d894e37936:
/***************************< File comment >***************************/ /* project: TurtleBot Project */ /* project description : - */ /*--------------------------------------------------------------------*/ /* version : 0.9 */ /* board : NUCLEO-F411RE */ /* detail : two hg is added */ /*--------------------------------------------------------------------*/ /* create : 19/08/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; Thread thread3; 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 fun */ /*--------------------------------------------------------------------*/ void IMU(); void servo(); void servoLeft(); void servoRight(); void servoFirstState(); void calcStepDown(float); void calcStepUp(float); bool checkIMUFirst(float , float, float ); /* debug func */ void printStateGait(); /*--------------------------------------------------------------------*/ /* Global variable */ /*--------------------------------------------------------------------*/ // home variable int initCheck = 0; float waittime = 0.001 ; int round = 25; // pm variable int iterPM = 0; float sumOfPower = 0.0, Energy = 0.0; // interface wt hormone variable /* // config flat // float upDeg = 45.00; float downDeg = 95.00; */ // config small // float upDeg = 60.00; float downDeg = 100.00; /* // config big // float upDeg = 75.00; float downDeg = 90.00; */ // servo motor variable float pos_down_start = 1400.00; float pos_up_start = 1000.00; float stepmin = 1.5; // servo left side float pos_down_left = 1400.00; float pos_up_left = 1000.00; float pos_down_end_left; float pos_up_end_left; 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 step_up_right; float step_down_right; int state_count_right = 0; int round_count_right = 0; // Timer variable uint32_t getIMUTimer; uint32_t walkingTimer; /* debug variable */ // print state gait int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0; /*--------------------------------------------------------------------*/ /* main */ /*--------------------------------------------------------------------*/ int main() { pc.baud(115200); 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 */ /*--------------------------------------------------------------------*/ void IMU() { 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; float readTime = 1; bool IMUWasStable = false; float *ArrayOfRoll = new float[10]; float *ArrayOfPitch = new float[10]; float *ArrayOfYaw = new float[10]; float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw; 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; /* memory allocate for G2 and G3 */ vAx = (float *)malloc(60*sizeof(float)); vAy = (float *)malloc(60*sizeof(float)); vAz = (float *)malloc(60*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(80*sizeof(float)); pPitch = (float *)malloc(80*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(); while(1) { if ((timer1.read_ms() - getIMUTimer) >= readTime) // read time in 1 ms { attitude_get(); /*--------------------------------------------------------------------*/ /* Signal Pre-Process every 10 ms */ /*--------------------------------------------------------------------*/ if(iterIMU < 10) { 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(); /* 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); /* 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 /* 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 = calc.calculateSD(ArrayOfRoll, 10); //SDOfRoll = calculateSD(ArrRoll, 10); //pc.printf("%.3f\t", SDOfRoll); //pc.printf("%.3f\t\t", ArrayOfRoll[9]); //////////// pitch /////////////// SDOfPitch = calc.calculateSD(ArrayOfPitch, 10); //SDOfPitch = calculateSD(ArrPitch, 10 ); //pc.printf("%.3f\t", SDOfPitch); //pc.printf("%.3f\t\t", ArrayOfPitch[9]); //////////// yaw /////////////// SDOfYaw = calc.calculateSD(ArrayOfYaw, 10); //SDOfYaw = calculateSD(ArrYaw, 10 ); //pc.printf("%.3f\n\r", SDOfYaw); //pc.printf("%.3f\t\t", ArrayOfYaw[9]); } else { iterPM++; /*--------------------------------------------------------------------*/ /* power monitoring in J */ /* PM ship can update fastest in 125 ms */ /*--------------------------------------------------------------------*/ //pc.printf("%d\t", iterPM); //pc.printf("%.3f\t", pmL1.Power()); //pc.printf("%.3f\t", pmR1.Power()); //pc.printf("%.3f\t", pmL2.Power()); //pc.printf("%.3f\n\r", pmR2.Power()); sumOfPower += pmL1.Power(); sumOfPower += pmR1.Power(); sumOfPower += pmL2.Power(); sumOfPower += pmR2.Power(); //sumOfPower = pmL1.Power() + pmR1.Power() + pmL2.Power() + pmR2.Power(); } if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming { 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"); delete[] ArrayOfRoll; delete[] ArrayOfPitch; delete[] ArrayOfYaw; readTime = 0.1; } /*--------------------------------------------------------------------*/ /* || 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) { // 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; //pc.printf("\n %.3f\t", pRoll[iterG4] ); //pc.printf("%.3f\n\r", pPitch[iterG4]); iterG4++; state_count_left_old = state_count_left; state_count_right_old = state_count_right; } else if(state_count_left_old == 4 and state_count_right_old == 4 and state_count_left == 1 and state_count_right == 1) { // 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); // normalize by 2 for SB downDeg = hg.downHG(sdVectorAG2, sdVectorAG3); pc.printf("%d\t", timer1.read_ms() - walkingTimer); pc.printf("%.3f\t", sumOfPower); 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); } /*--------------------------------------------------------------------*/ /* FIN for walking */ /*--------------------------------------------------------------------*/ if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round) { Energy = sumOfPower * ( (timer1.read_ms() - walkingTimer) / iterPM ); pc.printf("*\t"); pc.printf("%d\t", timer1.read_ms() - walkingTimer); pc.printf("%.3f\t", sumOfPower); 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)); pc.printf("TIME %d \n\r", timer1.read_ms() - walkingTimer); pc.printf("E %.3f \n\r", Energy / 1000.00f); pc.printf("P %.3f \n\r", sumOfPower); pc.printf("ITER PM %d \n\r", iterPM); pc.printf("FIN IMU!\n\r"); //thread1.terminate(); free(vAz);free(vAy);free(vAz); free(pRoll);free(pPitch); break; } // reset iteration iterIMU = 0; } getIMUTimer = timer1.read_ms(); } } } /*--------------------------------------------------------------------*/ /* servo */ /*--------------------------------------------------------------------*/ void servo() { hormone hr; Servo1.Enable(1000,20000); Servo2.Enable(1000,20000); Servo3.Enable(1000,20000); Servo4.Enable(1000,20000); Servo1.SetPosition(pos_down_left); Servo2.SetPosition(pos_up_left); Servo3.SetPosition(pos_down_right); Servo4.SetPosition(pos_up_right); while(1) { 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) { //pc.printf("FIN SERVO! \n\r"); //thread2.terminate(); break; } } } /*--------------------------------------------------------------------*/ /* calculate step of servo down */ /*--------------------------------------------------------------------*/ void calcStepDown(float hormDown) { 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 step_down_left = stepmin; } else if (pos_down_end_right < pos_down_end_left) { step_down_right = stepmin; step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); } else // pos_down_end_right == pos_down_end_left { step_down_right = stepmin; step_down_left = stepmin; } } /*--------------------------------------------------------------------*/ /* 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 step_up_left = stepmin; } else if (pos_up_end_right < pos_up_end_left) { step_up_right = stepmin; step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); } else // step_up_right == step_up_left { step_up_right = stepmin; step_up_left = stepmin; } } /*--------------------------------------------------------------------*/ /* servo in left side */ /*--------------------------------------------------------------------*/ void servoLeft() { if(state_count_left == 1) { Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00 wait(waittime); // 0.001 ms pos_down_left += step_down_left; if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) // pos_down_end_left ~ 1700 { state_count_left = 2; } } else if(state_count_left == 2) { Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00 wait(waittime); pos_up_left += step_up_left; if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { state_count_left = 3; } } else if(state_count_left == 3) { Servo1.SetPosition(pos_down_left); wait(waittime); pos_down_left -= step_down_left; if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { state_count_left = 4; } } else if(state_count_left == 4) { Servo2.SetPosition(pos_up_left); wait(waittime); 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 = 5; } } else if (state_count_left == 5 and round_count_left < round) { round_count_left++; state_count_left = 1; pos_down_left = pos_down_start; pos_up_left = pos_up_start; } } /*--------------------------------------------------------------------*/ /* servo in right side */ /*--------------------------------------------------------------------*/ void servoRight() { if(state_count_right == 1) { Servo3.SetPosition(pos_down_right); wait(waittime); pos_down_right = pos_down_right + step_down_right; if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) { state_count_right = 2; } } else if(state_count_right == 2) { Servo4.SetPosition(pos_up_right); wait(waittime); pos_up_right = pos_up_right + step_up_right; if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { state_count_right = 3; } } else if(state_count_right == 3) { Servo3.SetPosition(pos_down_right); wait(waittime); pos_down_right = pos_down_right - step_down_right; if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { state_count_right = 4; } } else if(state_count_right == 4) { Servo4.SetPosition(pos_up_right); wait(waittime); 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 = 5; } } else if (state_count_right == 5 and round_count_right < round) { round_count_right = round_count_right+1; state_count_right = 1; pos_down_right = pos_down_start; pos_up_right = pos_up_start; } } /*--------------------------------------------------------------------*/ /* 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; } /*--------------------------------------------------------------------*/ /* 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\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\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\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\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; } } /*--------------------------------------------------------------------*/