File content as of revision 61:bdc31e4fa3c4:

#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 "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();
    
    //timer.reset();
    timer.start();
    float timeWas = timer.read();
    
    servo_set_angle(0.0f);
    
    float speedRatio = 1.0f;
    float lastAngle = 0.0f;
    float cornerRatio = 1.0f;
    
    DebugCounter counter(10, PTE5);
    
    while (1) 
    {
        float deltaTime = timeWas;
        timeWas = timer.read();
        deltaTime = timeWas - deltaTime;
        
        //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;
        //}
        
        //ardu_cam_display_img_utft();
        image_processing();
        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;
        //////////////////////////////////////
        
        float totalAngleDegrees = ((angleDegrees * 0.50f) + (offsetDegrees * 0.35f));
        float totalAngleDegreesAbs = abs(totalAngleDegrees);
        servo_set_angle(totalAngleDegrees);
        
        if(totalAngleDegrees > SERVO_MAX_ANGLE)
            totalAngleDegrees = SERVO_MAX_ANGLE;
        else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
            totalAngleDegrees = -SERVO_MAX_ANGLE;
        
        if(totalAngleDegrees > lastAngle)
        {
            cornerRatio =  cornerRatio * (LasstAngle / totalAngleDegrees);
        }
        else if(totalAngleDegrees < lastAngle)
        {
            cornerRatio =  cornerRatio * (lastAngle / totalAngleDegrees);    
        }
        else
        {
            cornerRatio = 1.0f;    
        }
        
        if(totalAngleDegreesAbs > 18.0f)
        {
            speedRatio = 0.90f;
        }
        else if(totalAngleDegreesAbs > 15.0f)
        {
            speedRatio = 0.94f;
        }
        else if(totalAngleDegreesAbs > 12.0f)
        {
            speedRatio = 0.98f;
        }
        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;
        
        //LOGI("FPS: %f", 1 / deltaTime);
        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);
    }
}

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