turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 12:a9d894e37936
- Parent:
- 11:8548536c3f11
diff -r 8548536c3f11 -r a9d894e37936 main.cpp --- a/main.cpp Sun Aug 19 06:28:40 2018 +0000 +++ b/main.cpp Fri Aug 24 10:58:12 2018 +0000 @@ -3,11 +3,11 @@ /* project: TurtleBot Project */ /* project description : - */ /*--------------------------------------------------------------------*/ -/* version : 0.8 */ +/* version : 0.9 */ /* board : NUCLEO-F411RE */ /* detail : two hg is added */ /*--------------------------------------------------------------------*/ -/* create : 25/07/2018 */ +/* create : 19/08/2018 */ /* programmer : Worasuchad Haomachai */ /**********************************************************************/ @@ -56,16 +56,28 @@ // home variable int initCheck = 0; float waittime = 0.001 ; -float round = 15; +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; @@ -130,8 +142,11 @@ hormone hg; int iterIMU = 0, state_count_left_old = 0, state_count_right_old = 0; + float readTime = 1; bool IMUWasStable = false; - float ArrayOfRoll[10] = {0.000f}, ArrayOfPitch[10] = {0.000f}, ArrayOfYaw[10] = {0.000f}; + 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; @@ -149,9 +164,9 @@ float meanG4 = 0.0; /* memory allocate for G2 and G3 */ - vAx = (float *)malloc(50*sizeof(float)); - vAy = (float *)malloc(50*sizeof(float)); - vAz = (float *)malloc(50*sizeof(float)); + 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 ) { @@ -159,8 +174,8 @@ } /* memory allocate for G4 */ - pRoll = (float *)malloc(50*sizeof(float)); - pPitch = (float *)malloc(50*sizeof(float)); + pRoll = (float *)malloc(80*sizeof(float)); + pPitch = (float *)malloc(80*sizeof(float)); /* check malloc is not return NULL */ if( pRoll == NULL or pPitch == NULL ) { @@ -170,7 +185,7 @@ getIMUTimer = timer1.read_ms(); while(1) { - if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 ms + if ((timer1.read_ms() - getIMUTimer) >= readTime) // read time in 1 ms { attitude_get(); /*--------------------------------------------------------------------*/ @@ -269,6 +284,12 @@ walkingTimer = timer1.read_ms(); IMUWasStable = true; pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r"); + + delete[] ArrayOfRoll; + delete[] ArrayOfPitch; + delete[] ArrayOfYaw; + + readTime = 0.1; } /*--------------------------------------------------------------------*/ @@ -289,7 +310,7 @@ { // calculate SD of size vector A in G2 // sdVectorAG2 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG2); - //pc.printf("%.3f\t", sdVectorAG2); + pc.printf("%.3f\t", sdVectorAG2); // hormone concentration // //hg.hormoneCon(sdVectorAG2); @@ -318,7 +339,7 @@ { // calculate SD of size vector A in G3 // sdVectorAG3 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG3); - //pc.printf("%.3f\t", sdVectorAG3); + pc.printf("%.3f\t", sdVectorAG3); // hormone concentration // //hg.hormoneCon(sdVectorAG3); @@ -339,6 +360,8 @@ { 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; @@ -346,12 +369,15 @@ 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); + meanG4 = calc.calcG4(pRoll, pPitch, iterG4); + pc.printf("%.3f\t", meanG4); // hormone concentration // - upDeg = hg.upHG(sdVectorAG2, meanG4); + 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)); @@ -371,6 +397,9 @@ { 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));