Diff: main.cpp

Revision:

11:8548536c3f11

Parent:

8:865535fcf917

Child:

12:a9d894e37936

--- 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;     
         }