Diff: StateMachine/RunningState.cpp

Branch:

Drift

Revision:

80:c85cb93713b3

Child:

81:32bd7a25a699

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine/RunningState.cpp	Tue Apr 18 19:26:33 2017 +0000
@@ -0,0 +1,169 @@
+#include "RunningState.h"
+
+#include "StateManager.h"
+
+#include "ArduCAM.h"
+#include "ArduUTFT.h"
+#include "Motor.h"
+#include "Servo.h"
+
+#define PI 3.14159265f
+
+RunningState::RunningState() : 
+    m_speedRatio(1.0f),
+    m_lastAngle(0.0f),
+    m_lastAngleAbs(0.0f),
+    m_cornerRatio(1.0f),
+    m_lastLineFound(BOTH_FOUND)
+{}
+
+RunningState::~RunningState()
+{}
+
+void RunningState::DrawUserInterface()
+{
+    
+}
+
+void RunningState::Update(float deltaTime)
+{
+    image_processing();
+    uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
+    
+    if(shouldTerminate)
+    {
+        TerminateProcess();
+        return;
+    }
+    //else
+    //{
+    //    ardu_utft_print(" ", 230, 100);
+    //}
+    
+    const volatile uint8_t* centerLine = ardu_cam_get_center_array();
+    
+    /////////////Calculate the curvature:
+    float srcY = (0.0f + 1.0f) / 2.0f;
+    float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
+    
+    float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
+    float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
+    
+    float disY = destY - srcY;
+    float disX = destX - srcX;
+    
+    float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
+    float angleDegrees = (angleRadians * (180.0f / PI));
+    
+    ///////////////////////////////////////
+    
+    /////////////Calculate the offset:
+    float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
+    float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
+    //////////////////////////////////////
+    
+    const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
+    //const uint8_t lineFoundRefTop = ((2 * CAM_ROI_UPPER_LIMIT) - 1);
+    //const uint8_t lineFoundRefBut = (CAM_ROI_UPPER_LIMIT);
+    const uint8_t isBorderFound = ardu_cam_get_is_border_found();
+
+    float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
+    
+    if(totalAngleDegrees > SERVO_MAX_ANGLE)
+        totalAngleDegrees = SERVO_MAX_ANGLE;
+    else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
+        totalAngleDegrees = -SERVO_MAX_ANGLE;
+    
+    if((totalAngleDegrees < 0.0f && (isBorderFound == LEFT_FOUND)) ||
+    (totalAngleDegrees > 0.0f && (isBorderFound == RIGHT_FOUND)))
+    {
+        totalAngleDegrees = ((-1.0f) * totalAngleDegrees);// + ((0.2f) * m_lastAngle);
+    }
+    //LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
+    servo_set_angle(totalAngleDegrees);
+    
+    
+    float totalAngleDegreesAbs = abs(totalAngleDegrees);
+    
+    if(totalAngleDegreesAbs > m_lastAngleAbs)
+    {
+        m_cornerRatio =  0.4;//(m_lastAngle / totalAngleDegrees);
+    }
+    else if(totalAngleDegreesAbs < m_lastAngleAbs)
+    {
+        m_cornerRatio =  0.8;//(m_lastAngle / totalAngleDegrees);    
+    }
+    else
+    {
+        m_cornerRatio = 1.0f;    
+    }
+    
+    if(totalAngleDegreesAbs > 20.0f)
+    {
+        m_speedRatio = 0.350f;
+    }
+    else if(totalAngleDegreesAbs > 18.0f)
+    {
+        m_speedRatio = 0.80f;
+    }
+    else if(totalAngleDegreesAbs > 15.0f)
+    {
+        m_speedRatio = 0.90f;
+    }
+    else if(totalAngleDegreesAbs > 12.0f)
+    {
+        m_speedRatio = 0.92f;
+    }
+    else
+    {
+        m_speedRatio = 1.0f;
+    }
+    
+
+    if(totalAngleDegrees < 0.0f)
+    {
+        motor_set_left_speed(m_speedRatio * m_cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
+        motor_set_right_speed(m_speedRatio * m_cornerRatio * 1.0f);    
+    }
+    else
+    {
+        motor_set_right_speed(m_speedRatio * m_cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
+        motor_set_left_speed(m_speedRatio * m_cornerRatio * 1.0f);
+    }
+
+    m_lastAngle = totalAngleDegrees;
+    m_lastAngleAbs = totalAngleDegreesAbs;
+    m_lastLineFound = centerLine[lineFoundRefRow];
+}
+
+uint8_t RunningState::HasTouchPosFunction() const
+{
+    return 0;
+}
+
+uint8_t RunningState::HasTouchIrqFunction() const
+{
+    return 1;
+}
+
+void RunningState::TouchPosCallback(int16_t x, int16_t y)
+{
+    
+}
+
+void RunningState::TouchIrqCallback()
+{
+    TerminateProcess();
+}
+
+void RunningState::TerminateProcess()
+{
+    ardu_utft_print("F", 230, 100);
+    servo_set_angle(0.0f);
+    motor_set_right_speed(-1.0f);
+    motor_set_left_speed(-1.0f);
+    wait(0.4f);
+    motor_set_right_speed(0.0f);
+    motor_set_left_speed(0.0f);
+    state_manager_clear_current_state();
+}
\ No newline at end of file