File content as of revision 77:1ee12f434a23:

#include "mbed.h"

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

//#define SW_DEBUG
#include "SWCommon.h"
#include "GlobalVariable.h"

#include "Motor.h"
#include "Servo.h"
#include "WheelEncoder.h"
#include "Core.h"
#include "SWUSBServer.h"

#include "ArduCAM.h"
#include "ArduUTFT.h"
#include "IMUManager.h"

#include "PinAssignment.h"

SPI g_spi_port(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCK);
SW::Core g_core;

#ifdef SW_DEBUG
#include <rtos.h>
Mutex g_sw_spi_lock;
#endif 

int main(void) {
    
    Timer timer;
    
    g_spi_port.frequency(5000000);
    //g_spi_port.format(8, 0);
    
    //SW::Core core;
    
    //Motor motor(g_core);
    //Servo servo(g_core);
    //WheelEncoder wheelEncoder(core);
    //Camera cam(core);
    motor_init();
    servo_init();
    
    //bool isRegRead = false;
    
    
    ardu_utft_init();
    
    
    ardu_cam_init();
    
    
    //uint8_t IMUInitResult = imu_manager_init();
    //LOGI("IMU Init: %#x", IMUInitResult);
    //imu_manager_calibrate();
    //imu_manager_begin_tick();
    //wait(0.5);
    
    /*  DeltaTime Calculation
    timer.start();
    float timeWas = timer.read();
    */
    servo_set_angle(0.0f);
    
    float speedRatio = 1.0f;
    float lastAngle = 0.0f;
    float lastAngleAbs = 0.0f;
    float cornerRatio = 1.0f;
    uint8_t lastLineFound = BOTH_FOUND;
    
    //DebugCounter counter(10, PTE5);
    
    while (1) 
    {
    /*  DeltaTime Calculation
        float deltaTime = timeWas;
        timeWas = timer.read();
        deltaTime = timeWas - deltaTime;
    */
        
        
        //ardu_cam_display_img_utft();
        image_processing();
        uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
        
        if(shouldTerminate)
        {
            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);
            return 0;
        }
        //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);

        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 && lastAngle > 17.0f && lastLineFound != BOTH_FOUND && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
            (totalAngleDegrees > 0.0f && lastAngle < -17.0f && lastLineFound != BOTH_FOUND  && centerLine[lineFoundRefRow] == RIGHT_FOUND))
        {
            totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
        }
        LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
        servo_set_angle(totalAngleDegrees);
        
        
        float totalAngleDegreesAbs = abs(totalAngleDegrees);
        
        if(totalAngleDegreesAbs > lastAngleAbs)
        {
            cornerRatio =  0.4;//(lastAngle / totalAngleDegrees);
        }
        else if(totalAngleDegreesAbs < lastAngleAbs)
        {
            cornerRatio =  0.8;//(lastAngle / totalAngleDegrees);    
        }
        else
        {
            cornerRatio = 1.0f;    
        }
        
        if(totalAngleDegreesAbs > 20.0f)
        {
            speedRatio = 0.40f;
        }
        else if(totalAngleDegreesAbs > 18.0f)
        {
            speedRatio = 0.87f;
        }
        else if(totalAngleDegreesAbs > 15.0f)
        {
            speedRatio = 0.91f;
        }
        else if(totalAngleDegreesAbs > 12.0f)
        {
            speedRatio = 0.95f;
        }
        else
        {
            speedRatio = 1.0f;
        }
        
   
        if(totalAngleDegrees < 0.0f)
        {
            motor_set_left_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
            motor_set_right_speed(speedRatio * cornerRatio * 1.0f);    
        }
        else
        {
            motor_set_right_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
            motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
        }

        lastAngle = totalAngleDegrees;
        lastAngleAbs = totalAngleDegreesAbs;
        lastLineFound = centerLine[lineFoundRefRow];
    }
}

/*
PwmOut servo(PTE20);
 
int main() {
    servo.period(0.020);          // servo requires a 20ms period
    
    while (1) {
        for(float offset=0.0; offset<0.001; offset+=0.0001) {
            servo.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
            wait(0.25);
        }
    }
    
}
*/

/*  //code for accelerometer sensor.
    const char regAddr = 0x0D;
    char readValue = 0;
    int result1 = m_sccbCtrl.write(0x1D<<1, &regAddr, 1, true);
    int result2 = m_sccbCtrl.read(0x1D<<1, &readValue, 1, false);
    char buf[20];
    sprintf(buf, "%#x-%#x-%d-%d", regAddr, readValue, result1, result2);
    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
*/

        //g_core.Update(deltaTime);
        
        //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
        //{
            //ardu_cam_print_debug();
        //    isRegRead = true;
        //}

        //LOGI("FPS: %f", 1 / deltaTime);
        
        //imu_manager_init();
        //imu_manager_update();
        //float imuTemp = imu_manager_get_temp();
        
        //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
        
        //LOGI("A: %5.3f, %5.3f, %5.3f.T%5.2f ", AccelV->x, AccelV->y, AccelV->z, imuTemp);
        
        //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
        
        //LOGI("V: %5.3f, %5.3f, %5.3f.T%5.2f ", VelocityV->x, VelocityV->y, VelocityV->z, imuTemp);
        
        //const volatile struct imu_vec3* PositionV = imu_manager_get_position();
        
        //LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
        
        //counter.Update();
        
        /*
        //////// Steer Vehicle / Adjust Speed for Differential //////////
        servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
        
        if(angleRadians > 0.366)
            angleRadians = 0.366;
        else if(angleRadians < -0.366)
            angleRadians = -0.366;
        
        if(angleRadians < 0)
        {
            motor_set_left_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
            motor_set_right_speed(1.0);    
        }
        else
        {
            motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
            motor_set_left_speed(1.0);
        }
        */
        
        
        //char buf[20];
        //sprintf(buf, "angle %f", angle);
        //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
        /*
        std::string tempStr = "XX";
        for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
        {
            tempStr[0] = i;
            tempStr[1] = centerLine[i];
            g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
        }
        */
        
        /*
        if(offsetPercent > 0.1)
        {
            motor_set_speeds(0.05f, 0.05f);
        }
        else
        {
            motor_set_speeds(0.13f, 0.13f);
        }
        */
        //wait(0.01);