//#DOC Main, program entry and main loop
#include "main.h"
#include <otMath.h>
#include <otSerial.h>
#include <otTimer.h>
#include <otMalloc.h>
#include <otPort.h>
#include <otPrintf.h>
#include <otI2cBus.h>
#include <otMath.h>
#include <otFPGA.h>
#include <otPWM.h>
#include <otRFCom.h>
#include <otTFMini.h>
#include <otWS2812.h>
#include <otBotEngine.h>
// These parameters are related to the model used in the test
#define TrackWidth 265 // mm
// The underlying value was found experimentally using a LIDAR
#define EncoderStep 0.737463126f // mm x encoder count
#define Pi 3.14159265359f
// Convert a length to the number of encoder counts
#define Travel(x) ((float) (x) / EncoderStep)
// Space traveled for 1 degree rotation
#define AngleUnit (Pi * TrackWidth / 360.0f)
// Calculate the number of encoder counts based on the desired angle.
// Valid for rotation carried out with both motors.
#define Angle(x) Travel(((float) (x) * AngleUnit / 2.0f))
U16 _distance;
otPWM pwm;
otRFCom rf;
otTFMini tof;
otWS2812 ledPanel;
otBotEngine bot;
void setup()
{
UartEnableSignals(UART0);
UartSetBaud(UART0, 115200);
TimerSetAsCounter(TIMER0);
TimerEnable(TIMER0, true);
// Heap init
_initMalloc();
// printf output is now directed on UART0
setStdout(4096, UART0);
I2cBusReset();
I2cBusFrequency(100, 30);
if(pwm.init() == false)
{
printf("Error on PWM init\n");
IDLE;
}
pwm.setTimebase(25000000);
pwm.setPWMFreq(50);
bot.init(TIMER0, &pwm);
bot.setBrakeTime(20);
rf.init(UART0, PP_H, 4);
rf.setBaud(otRFCom::B_115200);
rf.apply();
ledPanel = otWS2812(12, 16, 16);
ledPanel.mirror(true, true);
ledPanel.setOffset(1);
tof.init(UART1);
}
void loop()
{
bool ok;
ledPanel.rectFill(0, 0, 15, 15, 5, 0, 0);
ledPanel.show();
DelayMs(1000);
ledPanel.rectFill(0, 0, 15, 15, 0, 0, 0);
ledPanel.show();
while(1)
{
if(UartCheckChar(UART0))
{
switch(UartChar(UART0))
{
case 'F': bot.action(otBotEngine::MA_FORWARD);
break;
case '>': bot.action(otBotEngine::MA_FORWARD, Travel(100));
break;
case 'B': bot.action(otBotEngine::MA_BACKWARD);
break;
case '<': bot.action(otBotEngine::MA_BACKWARD, Travel(100));
break;
case 'L': bot.action(otBotEngine::MA_TURNLEFT);
break;
case '[': bot.action(otBotEngine::MA_TURNLEFT, Angle(10));
break;
case 'R': bot.action(otBotEngine::MA_TURNRIGHT);
break;
case ']': bot.action(otBotEngine::MA_TURNRIGHT, Angle(10));
break;
case '.': bot.action(otBotEngine::MA_BRAKE);
break;
case '?': printf("Sx = %5d, Dx = %5d\n", bot.encoder(true), bot.encoder(false));
break;
case 'D': _distance = tof.getDistance(&ok);
if(ok)
printf("Dx = %5d cm\n", _distance);
break;
default:
printf("Unkonwn command!\n");
}
}
bot.process();
}
}