File content as of revision 44:15de535c4005:

#include "mbed.h"

#include <math.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"
#include "GlobalVariable.h"

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

int main(void) {
    
    Timer timer;
    
    g_spi_port.frequency(5500000);
    //g_spi_port.format(8, 0);
    
    //SW::Core core;
    
    Motor motor(g_core);
    Servo servo(g_core);
    //WheelEncoder wheelEncoder(core);
    //Camera cam(core);
    
    
    bool isRegRead = false;
    
    
    ardu_utft_init();
    
    
    ardu_cam_init();
    
    float tempCount = 0;
    //timer.reset();
    timer.start();
    float timeWas = timer.read();
    
    motor.setLeftSpeed(0.12f);
    motor.setRightSpeed(0.12f);
    while (1) 
    {
        
        float deltaTime = timeWas;
        timeWas = timer.read();
        deltaTime = timeWas - deltaTime;
        
        //led = 0;
        g_core.Update(deltaTime);
        motor.Update(deltaTime);
        servo.Update(deltaTime);
        //wheelEncoder.Update(deltaTime);
        //cam.Update(deltaTime);
        
        tempCount += deltaTime;
        if(tempCount > 1.0f)
        {
            //led = !led;
            tempCount = 0.0f;
        }
        
        
        if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
        {
            //OV7725RegBuf * regBuf = new OV7725RegBuf(g_core);
            //regBuf->ReadRegisters();
            //delete regBuf;
            //ardu_cam_print_debug();
            isRegRead = true;
        }
        
        //ardu_cam_display_img_utft();
        volatile const uint8_t * centerLine = ardu_cam_get_center_array();
        float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
        float offsetPercent = (static_cast<float>(centerLine[3]) - centerPos);
        offsetPercent = offsetPercent / centerPos;
        servo.setAngle(offsetPercent * 1.7f * SERVO_MAX_ANGLE);
        if(offsetPercent > 0.1)
        {
            motor.setLeftSpeed(0.05f);
            motor.setRightSpeed(0.05f);
        }
        else
        {
            motor.setLeftSpeed(0.13f);
            motor.setRightSpeed(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);
*/