Diff: main.cpp

Revision:

12:a9d894e37936

Parent:

11:8548536c3f11

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