File content as of revision 80:c85cb93713b3:

#include "mbed.h"

#include <math.h>


//#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 "ArduTouch.h"

#include "StateManager.h"
#include "RunningState.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();
    
    ardu_touch_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);

    
    //DebugCounter counter(10, PTE5);
    
    
    state_manager_set_current_state(new RunningState());
    
    while (1) 
    {
        state_manager_update(0.0f);
    }
}

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