int mainControlInit(void)
{
pid_loop.start_state = FALSE;
ROTATION_DIAMETER = sqrt(pow(WHEELS_DISTANCE, 2) + pow(WHEELS_SPACING, 2));
motorsInit();
encodersInit();
telemetersInit();
adxrs620Init();
speedControlInit();
positionControlInit();
wallFollowControlInit();
lineFollowControlInit();
transfertFunctionInit();
positionControlSetPositionType(ENCODERS);
mainControlSetFollowType(NO_FOLLOW);
mainControlSetMoveType(STRAIGHT);
control_params.wall_follow_state = 0;
control_params.line_follow_state = 0;
return MAIN_CONTROL_E_SUCCESS;
}
int mainControlInit(void)
{
ROTATION_DIAMETER = sqrt(pow(WHEELS_DISTANCE, 2) + pow(WHEELS_SPACING, 2));
motorsInit();
encodersInit();
mulimeterInit();
telemetersInit();
speedControlInit();
positionControlInit();
wallFollowControlInit();
lineFollowControlInit();
transfertFunctionInit();
wallSensorInit();
adxrs620Init();
speed_params.initial_speed = 0;
control_params.wall_follow_state = 0;
control_params.position_state = 0;
control_params.speed_state = 0;
control_params.line_follow_state = 0;
return MAIN_CONTROL_E_SUCCESS;
}
void mainInit()
{
// not used pins as inputs with pull-ups
DDRB &= ~(1 << PB4);
PORTB |= (1 << PB4);
DDRB &= ~(1 << PB5);
PORTB |= (1 << PB5);
s_timerCounter = 0;
s_started = 0;
s_buttonPressed = 0;
s_buttonLock = 0;
s_speed = 40;
s_straightMode = CONTROLLER_LEFT_AND_RIGHT;
sei();
motorsInit();
wheelsInit();
controllerInitWithTimer(&mainTimerTick);
remoteInit();
sensorsInit();
ledInit();
buttonInit();
}
void hardwareInit(void)
{
PLLConfig();
SET_ADPCFG;
buttonsInit();
LED_SET_OUT;
LED_OFF;
sensorsInit();
sensorsOn();
readBatteryVoltage();
servoInit();
motorsInit();
//motorsOff();
profilerInit();
readButtons();
if( key_UP&&key_DN ) //if ICSP is connected
{
displayEnabled = TRUE;
serialInit();
//_TRISB10 = 0;
//_TRISB11 = 0;
}
CC2500_init();
systemTimerInit();
}
void GeekBot::init()
{
pinMode(LED_LEFT_PIN, OUTPUT);
pinMode(LED_RIGHT_PIN, OUTPUT);
digitalWrite(LED_LEFT_PIN, HIGH);
digitalWrite(LED_RIGHT_PIN, HIGH);
motorsInit( LEFT_SERVO_PIN, RIGHT_SERVO_PIN );
navigationInit( &mySounds );
lineFollowerInit( &mySounds );
LineSensorArrayInit();
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
chEvtInit(&eventImuIrq);
chEvtInit(&eventMagnIrq);
chEvtInit(&eventImuRead);
chEvtInit(&eventMagnRead);
chEvtInit(&eventEKFDone);
palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );
I2CInitLocal();
configInit();
mavlinkInit();
initSensors();
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_Cmd(TIM5, ENABLE);
startEstimation();
startSensors();
radioInit();
motorsInit();
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, 0);
extChannelEnable(&EXTD1, 1);
chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);
while (TRUE) {
chThdSleepMilliseconds(1000);
}
}
int main()
{
I2CCallbacks.onReadFunction = onI2CRead;
I2CCallbacks.onWriteFunction = onI2CWrite;
sei();
motorsInit();
while(true);
return 0;
}
void stabilizerInit(void) {
if (isInit)
return;
motorsInit();
imu6Init();
sensfusion6Init();
controllerInit();
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const )"STABILIZER",
2*configMINIMAL_STACK_SIZE, NULL, /*Piority*/2, NULL);
isInit = TRUE;
}
int main(void) {
ledsInit();
timerInit();
//move interruptvectors to the Boot sector
// this strange sequence ( two separate lines ) is necessary
MCUCR = _BV(IVCE);
MCUCR = _BV(IVSEL);
sei();
motorsInit();
uartInit( UART_BAUD_SELECT( 115200, F_CPU ) );
uartFlowControlOn( 0 );
uartPutStringCRLF( PROMPT );
while(1) {
// if more than a second has elapsed
if ( s_timeoutExpired && !s_stayInBootLoader ) {
jumpToApplication();
}
if ( getLine() ) {
if ( !strcmp( s_buffer, UPLOAD_PAGE_COMMAND ) ) {
s_stayInBootLoader = 1;
uploadPage();
} else if ( !strcmp( s_buffer, CHECK_PAGE_COMMAND ) ) {
s_stayInBootLoader = 1;
checkPage();
} else if ( !strcmp( s_buffer, QUIT_COMMAND ) ) {
jumpToApplication();
} else if ( !strcmp( s_buffer, "" ) ) {
s_stayInBootLoader = 1;
// don't do anything for blank lines
} else {
uartPutStringCRLF( CMD_UNKNOWN );
}
uartPutStringCRLF( PROMPT );
}
}
return 0;
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit();
imu6Init();
sensfusion6Init();
controllerInit();
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const)STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit(motorMapDefaultBrushed);
imu6Init();
sensfusion6Init();
controllerInit();
rollDesired = 0;
pitchDesired = 0;
yawDesired = 0;
xTaskCreate(stabilizerTask, STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit(motorMapBrushed);
imu6Init();
sensfusion6Init();
controllerInit();
#if defined(SITAW_ENABLED)
sitAwInit();
#endif
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const)STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
static void bigquadInit(DeckInfo *info)
{
if(isInit)
return;
DEBUG_PRINT("Switching to brushless.\n");
motorsInit(motorMapBigQuadDeck);
extRxInit();
#ifdef BQ_DECK_ENABLE_PM
pmEnableExtBatteryVoltMeasuring(BIGQUAD_BAT_VOLT_PIN, BIGQUAD_BAT_VOLT_MULT);
pmEnableExtBatteryCurrMeasuring(BIGQUAD_BAT_CURR_PIN, BIGQUAD_BAT_AMP_PER_VOLT);
#endif
#ifdef BQ_DECK_ENABLE_OSD
uart1Init(115200);
mspInit(&s_MspObject, osdResponseCallback);
xTaskCreate(osdTask, "BQ_OSDTASK",
configMINIMAL_STACK_SIZE, NULL, /*priority*/1, NULL);
#endif
isInit = true;
}
int test_move_zhonx ()
{
labyrinthe maze;
positionRobot zhonx_position;
int max_speed_rotation = SAFE_SPEED_ROTATION;
int max_speed_translation = SAFE_SPEED_TRANSLATION;
int min_speed_translation = SAFE_SPEED_TRANSLATION;
zhonx_position.coordinate_robot.x = 8;
zhonx_position.coordinate_robot.y = 8;
zhonx_position.midOfCell = TRUE;
zhonx_position.orientation = NORTH;
coordinate way[]={{8,7},{8,6},{8,5},{8,4},{8,3},{8,2},{8,1},{8,0},{9,0},{10,0},{11,0},{10,0},{9,0},{8,0},{8,1},{8,2},{8,3},{8,4},{8,5},{8,6},{8,7},{8,8},{END_OF_LIST,END_OF_LIST}};
motorsInit();
HAL_Delay(2000);
telemetersStart();
mainControlSetFollowType(WALL_FOLLOW);
positionControlSetPositionType(GYRO);
moveRealZhonxArc(&maze, &zhonx_position, way, max_speed_rotation, max_speed_translation, min_speed_translation);
HAL_Delay(500);
motorsDriverSleep(ON);
telemetersStop();
return MAZE_SOLVER_E_SUCCESS;
}
void powerDistributionInit(void)
{
motorsInit(motorMapDefaultBrushed);
}
