turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 11:8548536c3f11
- Parent:
- 8:865535fcf917
- Child:
- 12:a9d894e37936
diff -r 1f38c3e753b4 -r 8548536c3f11 main.cpp --- a/main.cpp Mon Aug 13 11:44:42 2018 +0000 +++ b/main.cpp Sun Aug 19 06:28:40 2018 +0000 @@ -28,6 +28,7 @@ Timer timer1; Thread thread1; Thread thread2; +Thread thread3; Servo Servo1(D4); // Servo Left Up (L1) Servo Servo2(D5); // Servo Left Down (L2) @@ -55,8 +56,12 @@ // home variable int initCheck = 0; float waittime = 0.001 ; -float round = 6; +float round = 15; +// pm variable +int iterPM = 0; +float sumOfPower = 0.0, Energy = 0.0; + // interface wt hormone variable float upDeg = 45.00; float downDeg = 95.00; @@ -86,7 +91,7 @@ int state_count_right = 0; int round_count_right = 0; -// other variable +// Timer variable uint32_t getIMUTimer; uint32_t walkingTimer; @@ -106,7 +111,7 @@ { timer1.start(); // start timer counting thread1.start(IMU); // IMU thread start - thread2.start(servo); // servo thread start + thread2.start(servo); // servo thread start } } @@ -115,6 +120,7 @@ /*--------------------------------------------------------------------*/ void IMU() { + powerMon pmR2(-1); powerMon pmR1(2000); powerMon pmL2(560); @@ -198,8 +204,8 @@ //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", 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 */ @@ -230,6 +236,24 @@ //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 { @@ -246,9 +270,6 @@ IMUWasStable = true; pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r"); } - /*--------------------------------------------------------------------*/ - /* power monitoring in J */ - /*--------------------------------------------------------------------*/ /*--------------------------------------------------------------------*/ /* || A || in State 2 */ @@ -348,8 +369,18 @@ 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("%.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("FIN! \n\r"); + 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); @@ -391,8 +422,7 @@ // 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("TIME %d \n\r", timer1.read_ms() - walkingTimer); - //pc.printf("FIN! \n\r"); + //pc.printf("FIN SERVO! \n\r"); //thread2.terminate(); break; }