Diff: main.cpp

Revision:

44:15de535c4005

Parent:

43:0d1886f4848a

Child:

45:501b7909139a

--- a/main.cpp	Mon Mar 27 22:09:22 2017 +0000
+++ b/main.cpp	Wed Mar 29 21:19:27 2017 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+#include <math.h>
+
 #include "Motor.h"
 #include "Servo.h"
 #include "WheelEncoder.h"
@@ -19,13 +21,13 @@
     
     Timer timer;
     
-    g_spi_port.frequency(7000000);
+    g_spi_port.frequency(5500000);
     //g_spi_port.format(8, 0);
     
     //SW::Core core;
     
-    //Motor motor(core);
-    //Servo servo(core);
+    Motor motor(g_core);
+    Servo servo(g_core);
     //WheelEncoder wheelEncoder(core);
     //Camera cam(core);
     
@@ -42,6 +44,9 @@
     //timer.reset();
     timer.start();
     float timeWas = timer.read();
+    
+    motor.setLeftSpeed(0.12f);
+    motor.setRightSpeed(0.12f);
     while (1) 
     {
         
@@ -51,8 +56,8 @@
         
         //led = 0;
         g_core.Update(deltaTime);
-        //motor.Update(deltaTime);
-        //servo.Update(deltaTime);
+        motor.Update(deltaTime);
+        servo.Update(deltaTime);
         //wheelEncoder.Update(deltaTime);
         //cam.Update(deltaTime);
         
@@ -64,16 +69,31 @@
         }
         
         
-        if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_get_fifo_length() > 0)
+        if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
         {
             //OV7725RegBuf * regBuf = new OV7725RegBuf(g_core);
             //regBuf->ReadRegisters();
             //delete regBuf;
-            ardu_cam_print_debug();
+            //ardu_cam_print_debug();
             isRegRead = true;
         }
         
-        ardu_cam_display_img_utft();
+        //ardu_cam_display_img_utft();
+        volatile const uint8_t * centerLine = ardu_cam_get_center_array();
+        float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
+        float offsetPercent = (static_cast<float>(centerLine[3]) - centerPos);
+        offsetPercent = offsetPercent / centerPos;
+        servo.setAngle(offsetPercent * 1.7f * SERVO_MAX_ANGLE);
+        if(offsetPercent > 0.1)
+        {
+            motor.setLeftSpeed(0.05f);
+            motor.setRightSpeed(0.05f);
+        }
+        else
+        {
+            motor.setLeftSpeed(0.13f);
+            motor.setRightSpeed(0.13f);
+        }
         
         //wait(0.01);
     }