int main(int argc, char **argv) {
Motor_init();
Sensor_init();
Button_init();
Compass_init();
turnCount = 0;
while(1) {
int front = (Sensor_getDistance(Sensor_FRONT_LEFT) +
Sensor_getDistance(Sensor_FRONT_RIGHT)) / 2;
while(front > 20) {
if(Compass_inRange(startDir, 15, 180))
Motor_steerLeft(0.9);
else if(Compass_inRange(startDir, 180, 45))
Motor_steerRight(0.9);
else
Motor_forward();
}
hugObstacle();
}
Motor_cleanUp();
Sensor_cleanUp();
Button_cleanUp();
}
void BLDC3PhaseSensor::main_test( void )
{
Motor_init();
//Set_Speed(speed);
//Set_Direction(direction);
for(;;) // update speed and direction
{
Run_motor();
_delay_ms(1000);
}
}
int main (void)
{
//Set Pin Modes
// pinMode(PIN_RaspiReset, OUTPUT);
// pinMode(PIN_IHTempSensor, INPUT/*_ANALOG*/);
// pinMode(PIN_MotorPosSensor, INPUT_PULLUP);
pinMode(PIN_Ventil, OUTPUT);
// pinMode(PIN_SafetyChain, OUTPUT);
pinMode(PIN_PusherLocked, INPUT_PULLUP);
pinMode(PIN_LidLocked, INPUT_PULLUP);
pinMode(PIN_LidClosed, INPUT_PULLUP);
// pinMode(PIN_SimulateButton0, OUTPUT);
// pinMode(PIN_IHOff, OUTPUT);
// pinMode(PIN_isIHOn, INPUT_PULLUP);
// pinMode(PIN_IHOn, OUTPUT);
// pinMode(PIN_MotorPWM, OUTPUT/*_PWM*/);
// pinMode(PIN_IHPowerPWM, OUTPUT/*_PWM*/);
// pinMode(PIN_IHFanPWM, OUTPUT/*_PWM*/);
Motor_init();
Heating_init();
initTime();
//Init LoL-Shield
// #ifdef GRAYSCALE
LedSign_Init(DOUBLE_BUFFER | GRAYSCALE); //Initializes the screen
// #else
// LedSign_Init(DOUBLE_BUFFER); //Initializes the screen
// #endif
LedSign_Clear(0);
LedSign_Flip(false);
//initialize time messurement
//initTime();
//Init SPI
SPI_init(wdtRestart);
//Disable JTAG interface
MCUCR = _BV(JTD);
MCUCR = _BV(JTD); //Need to write twice to disable it (Atmega_644.pdf, page 267, 23.8.1)
#ifdef DEBUG_MODE
if (isExternalReset){
wdtRestartCount = wdtRestartCount + 1;
}
//TODO remove, test was WTD reset
if (wdtRestart && wdtRestartLast){
wdtRestart = false;
}
wdtRestartLast = wdtRestart;
digitalWrite(PIN_Ventil, wdtRestart);
wdtRestart = false;
char valueAsString[10];
sprintf(valueAsString, "R%X", wdtRestartCount); //HEX
//sprintf(valueAsString, "%d", wdtRestartCount); //DEC
DisplayHandler_setText2(valueAsString);
DisplayHandler_displayText(false);
_delay_ms(50);
#endif
LedSign_Clear(PIXEL_HALF);
LedSign_Flip(true);
_delay_ms(100);
LedSign_Clear(PIXEL_OFF);
LedSign_Flip(true);
/*
digitalWrite(PIN_Ventil, HIGH);
_delay_ms(5000);
digitalWrite(PIN_Ventil, LOW);
*/
if (isStartup){
//Was power On reset
LedSign_Clear(PIXEL_OFF);
LedSign_Flip(true);
DisplayHandler_setText("EveryCook is starting");
currentMode = SPI_MODE_DISPLAY_TEXT;
} else {
//is other than power on reset
DisplayHandler_setPicture(&picture_hi[0]);
DisplayHandler_DisplayBitMap();
}
initWatchDog();
while(1){
Motor_motorControl();
Heating_heatControl();
Heating_controlIHTemp();
checkLocks();
if (!LidClosed) Motor_setMotor(0);
if (availableSPI() > 0) {
triggerWatchDog(true);
uint8_t data;
uint8_t newMode = readSPI(true);
char* newText;
uint8_t readAmount = 0;
boolean setOn = false;
boolean setOff = false;
switch(newMode){
case SPI_MODE_IDLE:
//DisplayHandler_setPicture(&picture_0[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_STATUS:
nextResponse = StatusByte;
//DisplayHandler_setPicture(&picture_1[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_DISPLAY_CLEAR:
LedSign_Clear(0);
LedSign_Flip(true);
currentMode = 0;
nextResponse = SPI_CommandOK;
break;
case SPI_MODE_MOTOR:
data = readSPI(true);
wdt_reset();
if(LidClosed){
Motor_setMotor(data);
}else{
Motor_setMotor(0);
}
nextResponse = SPI_CommandOK;
//DisplayHandler_setPicture(&picture_2[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_HEATING:
data = readSPI(true);
wdt_reset();
Heating_setHeating(data);
nextResponse = SPI_CommandOK;
//DisplayHandler_setPicture(&picture_3[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_DEBUG:
nextResponse = Vdebug;
//DisplayHandler_setPicture(&picture_18[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_VENTIL:
VentilState = readSPI(true);
wdt_reset();
digitalWrite(PIN_Ventil, VentilState);
nextResponse = SPI_CommandOK;
//DisplayHandler_setPicture(&picture_10[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_DISPLAY_PERCENT:
case SPI_MODE_DISPLAY_PERCENT_TEXT:
data = readSPI(true);
wdt_reset();
DisplayHandler_displayProgress(data, newMode == SPI_MODE_DISPLAY_PERCENT_TEXT);
lastPercentValue = data;
if (newMode != SPI_MODE_DISPLAY_PERCENT_TEXT){
nextResponse = SPI_CommandOK;
currentMode = newMode;
break;
}
case SPI_MODE_DISPLAY_TEXT:
case SPI_MODE_DISPLAY_TEXT_SMALL:
if (DisplayHandler_readText()) {//[01]<textlen 1 Byte><text>[00<control byte>]
if (newMode == SPI_MODE_DISPLAY_TEXT){
DisplayHandler_displayText(false);
} else {
DisplayHandler_displayText(true);
}
lastTextUpdate = millis();
if (newMode == SPI_MODE_DISPLAY_PERCENT_TEXT){
updatePercentAgain = true;
}
nextResponse = SPI_CommandOK;
currentMode = newMode;
} else {
nextResponse = SPI_Error_Text_Invalid;
}
break;
case SPI_MODE_DISPLAY_PICTURE:
readAmount=0;
while (readAmount < 9){
if (availableSPI() >= 2) {
data = readSPI(true);
picture[readAmount] = data << 8;
data = readSPI(true);
picture[readAmount] |= data;
readAmount++;
wdt_reset();
} else {
_delay_ms(1);
}
}
if (availableSPI() == 0) {
_delay_ms(10);
if (availableSPI() == 0) {
_delay_ms(20);
}
}
if (availableSPI() > 0 && peekSPI() == 0x00) {
readSPI(false);
DisplayHandler_setPicture(&picture[0]);
DisplayHandler_DisplayBitMap();
currentMode = newMode;
nextResponse = SPI_CommandOK;
} else {
DisplayHandler_setPicture(&picture[0]);
DisplayHandler_DisplayBitMap();
currentMode = newMode;
nextResponse = SPI_Error_Picture_Invalid;
}
break;
case SPI_MODE_MAINTENANCE:
data = readSPI(true);
if (data == 0x99) {
setOn = true;
} else if (data == 0x22){
setOff = true;
}
//be sure its change to maintenance, it has the be send 2 times
data = readSPI(true);
if (data == SPI_MODE_MAINTENANCE) {
data = readSPI(true);
if (setOn && data != 0x99) {
setOn = false;
} else if (setOff && data != 0x22){
setOff = false;
}
data = readSPI(true);
//00 as command end mark
if (data == 0x00){
if (setOn){
isMaintenanceMode = true;
nextResponse = SPI_CommandOK;
} else if (setOff){
isMaintenanceMode = false;
nextResponse = SPI_CommandOK;
} else {
nextResponse = SPI_Error_Unknown_Command;
}
} else {
nextResponse = SPI_Error_Unknown_Command;
}
} else {
nextResponse = SPI_Error_Unknown_Command;
}
break;
case SPI_MODE_GET_MOTOR_SPEED:
nextResponse = outputValueMotor;
//DisplayHandler_setPicture(&picture_12[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_IGBT_TEMP:
nextResponse = ihTemp8bit;
//DisplayHandler_setPicture(&picture_hi[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_FAN_PWM:
nextResponse = lastIHFanPWM;
//DisplayHandler_setPicture(&picture_17[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_HEATING_OUTPUT_LEVEL:
nextResponse = 0;
//nextResponse = Heating_getLastOnPWM();
//DisplayHandler_setPicture(&picture_14[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_MOTOR_POS_SENSOR:
nextResponse = lastSensorValue;
//DisplayHandler_setPicture(&picture_15[0]);
//DisplayHandler_DisplayBitMap();
break;
case SPI_MODE_GET_MOTOR_RPM:
nextResponse = rpm;
//DisplayHandler_setPicture(&picture_16[0]);
//DisplayHandler_DisplayBitMap();
break;
default:
nextResponse = SPI_Error_Unknown_Command;
}
} else {
triggerWatchDog(false);
switch(currentMode){
case SPI_MODE_IDLE:
break;
case SPI_MODE_DISPLAY_PERCENT_TEXT:
if (updatePercentAgain) {
DisplayHandler_displayProgress(lastPercentValue, true);
updatePercentAgain = false;
}
case SPI_MODE_DISPLAY_TEXT:
case SPI_MODE_DISPLAY_TEXT_SMALL:
if ((millis() - lastTextUpdate) > TEXT_UPDATE_TIMEOUT){
lastTextUpdate = millis();
if(!DisplayHandler_displayText(currentMode != SPI_MODE_DISPLAY_TEXT)){
currentMode = 0;
}
}
break;
}
//DisplayHandler_setPicture(&picture_0[0]);
//DisplayHandler_DisplayBitMap();
}
}
return 0;
}
int main(void)
{
// u8 a[] = {0xAA, 0xAA, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88,0x99,0xa0,0x04,0x10,0x08,0x01,0x08,0x99};
u8 a[] = {0x11, 0x22, 0x33,0x44, 0x55, 0x66, 0x77};
RobotRate rate;
WheelSpeed wheelspeed;
SystemInit();
USART1_Init(115200);
USART2_Init(115200);
USART3_Init(38400);
UART4_Init(115200);
CAN1_Init();
LED_Init();
// TIM2_Init();
TIM3_Init();
SysTick_Init();
Motor_init();
amp_init();
mag_sensor_init();
flash_init();
DelayMs(1000); //Time for Motor Driver Board to init
//set_all_speedctl();
t3 = micros();
//*************************initial sensor***************************************************************//
while(t < 0x15)
{
if(UART4RecvPtrR != UART4RecvPtrW)
{
op = AHRSCheckDataFrame();
if(op == ACC_METER || op == GYRO || op == ANGLE_OUTPUT || op == MAG_METER )
{
SensorInitial(op);
t++;
}
}
t4 = micros();
time_taken = t4 - t3;
if(time_taken > 3000000)
{
//break;
}
}
sch_init();
sch_add_task(sensors, 6, 20);
sch_add_task(AHRS_compute, 1, 50);
// sch_add_task(led_task, 4, 100);
sch_add_task(UART2Proc, 10, 20);
// sch_add_task(UART3Proc, 3, 20);
// sch_add_task(UART3Proc, 4, 20);
// sch_add_task(FRIDCheck, 2, 20);
sch_start();
while (1)
{
sch_dispatch_tasks();
//Welcome();
}
}
void main( void )
{
WDT_init();
UCS_init();
TimerA1_init();
TimerB0_init();
UART_init(UCA1,115200);
RotaryEncoder_init();
keyboard_init();
// GUI_init();
Motor_init();
{
RP.myOutput = 0;
RP.myInput = &E1_absPulseCnt;
RP.mySetpoint = 0;
RP.inAuto = 1;
PID_setOutputLimits(&RP, (signed long)-100, (signed long)100);//(加速度)
// PID_setOutputLimits(&RP, (signed long)-600, (signed long)600);
RP.SampleTime = 5;
PID_setControllerDirection(&RP, 1);
PID_setTunings(&RP, 184, 7000, 34); //400,0,0 //184,6000,35
if (TimeBase>RP.SampleTime)
RP.lastTime = TimeBase-RP.SampleTime;
else
RP.lastTime = 0;
}
_EINT();
// while(UCA1_GET_CHAR(&command));
_DINT();
E1_pulseCnt=2000;
E2_pulseCnt=0;
E1_pulseInLastMs=2000;
_EINT();
startTime = TimeBase;
// Motor_config(362);
// Clear_Screen();
// TFT_Menu_StartMenu();
while(function_3());
/*
while(1)
{
//主要任务
switch(function)
{
case 1:
break;
case 2:
break;
case 3:
break;
case 4:
break;
case 5:
break;
case 6:
if (E1_absPulseCnt==0)
{
function6Flag=1;
function6_E2_pulseCnt=E2_pulseCnt;
function6_speed=400;
}
if (function6Flag)
{
// if((!PID_compute(&RP))&&(abs(E2_pulseCnt-function6_E2_pulseCnt)<2000))
{
speed+=RP.myOutput;
speed-=E2_interval;
if (speed>0)
speedSign = 1;
else if (speed<0)
speedSign = -1;
else
speedSign = 0;
if (abs(speed)>900)
speed=speedSign*900;
if (speedSign>0)
Motor_config(speedSign*400+speed/3+5);
else
Motor_config(speedSign*400+speed/3+5);
// }
if ((abs(E1_absPulseCnt)<500)&&(abs(E1_absPulseCnt)>2))
{
RP.inAuto=1;
// PID_setMode(&RP,AUTOMATIC);
if(!PID_compute(&RP))
{
speed+=RP.myOutput;
speed-=E2_interval*3;
if (speed>0)
speedSign = 1;
else if (speed<0)
speedSign = -1;
else
speedSign = 0;
if (abs(speed)>900)
speed=speedSign*900;
if (speedSign>0)
{
Motor_config(speedSign*400+speed/3+5);
}
else
{
Motor_config(speedSign*400+speed/3+5);
}
}
if(abs(E2_pulseCnt-function6_E2_pulseCnt)>2000)
{
function6Flag=0;
Motor_config(0);
}
}
}
else
{
Motor_config(0);
if ((abs(E1_absPulseCnt)>1990)&&(function6Flag))
{
function6Flag=0;
}
}
break;
default:
break;
}
}*/
}
void Kernel_run()
{
//control flags
bool done = false;
bool error = false;
//message handling variables
int srcPID;
unsigned int size;
int error_code;
// Setup gpio and clocking
Kernel_init();
//task inits
WiFi_init();
WiFi_controller_init();
Motor_init();
//Kernel_register_function(dummy_function);
Kernel_register_function(WiFi_run);
Kernel_register_function(WiFi_controller_run);
Kernel_register_function(Motor_run);
while(done == false)
{
Kernel_task_complete();
//execute next task
if(num_tasks > 0)
{
(*schedule[current_task])();
}
do
{
//check for errors
error = MPI_check_available(OS_PID);
if(error == true)
{
//retrieve and handle errors
MPI_get_message(OS_PID, &srcPID, &size, &error_code);
}
//handle errors
if(size == sizeof(int))
{
switch(error_code)
{
case E_MPI_INVALID_SRC_PID:
case E_MPI_INVALID_DST_PID:
case E_MPI_OVERSIZED_MSG:
case E_MPI_MAILBOX_BUF_OVERFLOW:
case E_MPI_INVALID_RCV_PID:
case E_MPI_NO_MSG_AVAILABLE:
case E_KERNEL_TASK_OVERFLOW:
break;
//immediately terminate kernel execution
case E_KERNEL_TERMINATE:
done = true;
error = false;
break;
}
}
}while(error == true);
current_task = (current_task+1)%num_tasks;
}
}
