File content as of revision 86:51048c1f132f:

#include "RunningState.h"

#include "StateManager.h"

#include "ArduCAM.h"
#include "ArduUTFT.h"
#include "Motor.h"
#include "Servo.h"

#define PI 3.14159265f
#include <math.h>

RunningState::RunningState() : 
    m_speedRatio(1.0f),
    m_lastAngle(0.0f),
    m_lastAngleAbs(0.0f),
    m_cornerRatio(1.0f),
    //m_lastLineFound(BOTH_FOUND),
    m_shouldTerminate(0)
{}

RunningState::~RunningState()
{}

void RunningState::DrawUserInterface()
{
    ardu_utft_clr_scr();
    
    ardu_utft_set_color(255, 0, 0);
    ardu_utft_fill_rect(10, 10, 310, 230);
    ardu_utft_set_color(255, 255, 255);
    ardu_utft_print("STOP", 170, 120);
}

void RunningState::Update(float deltaTime)
{
    image_processing();
    uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
    
    if(shouldTerminate || m_shouldTerminate)
    {
        m_shouldTerminate = 0;
        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 = ((-0.2f) * 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::TouchIrqCallback()
{
    //TerminateProcess();
    m_shouldTerminate = 1;
}

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_switch_state(STANDBY_STATE);
}