void HardFault_Handler(void)
{
telemetersStop();
motorsDriverSleep(ON);
motorsBrake();
tone(A4, 4000);
halt();
}
void mainControlTest(void)
{
mainControlInit();
telemetersStart();
HAL_Delay(500);
wall_follow_control.follow_type = NOFOLLOW;
rotate180WithCal(CCW, 400, 0);
telemetersStop();
mainControlDisplayTest();
}
void followWallTest()
{
mainControlInit();
telemetersStart();
motorsSleepDriver(OFF);
HAL_Delay(500);
// control_params.wall_follow_state = TRUE;
wall_follow_control.follow_type = FOLLOW_WALL;//NOFOLLOW
// moveUTurn(300, 300, 0);
//while(1);
move(0, 0, 0, 0);
moveStartCell(500, 500);
moveCell(5, 500, 300);
moveRotateCW90(300, 300);
moveCell(3, 500, 300);
moveRotateCW90(300, 300);
moveCell(2, 500, 300);
moveRotateCW90(300, 300);
moveCell(2, 500, 300);
moveRotateCCW90(300, 300);
moveCell(1, 500, 300);
moveRotateCW90(300, 0);
// moveRotateCW90(50, 10);
// moveRotateCCW90(50, 10);
// moveRotateCCW90(50, 10);
while(1);
moveRotateCW90(600, 200);
moveRotateCW90(600, 200);
moveCell(1, 600, 200);
moveRotateCCW90(600, 200);
moveRotateCCW90(600, 200);
HAL_Delay(1000);
moveCell(4, 1000, 200);
HAL_Delay(1000);
moveRotateCW90(600, 200);
moveRotateCCW90(600, 200);
moveRotateCW90(600, 200);
moveRotateCCW90(600, 200);
moveRotateCW90(600, 200);
moveRotateCCW90(600, 200);
moveRotateCW90(600, 200);
moveCell(1, 600, 200);
telemetersStop();
mainControlDisplayTest();
mainControlDisplayTest();
}
void testPostSensors()
{
telemetersInit();
telemetersStart();
while (expanderJoyFiltered() != JOY_LEFT)
{
ssd1306ClearScreen(MAIN_AREA);
if (fabs(getTelemeterSpeed(TELEMETER_DL)) > 500)
{
ssd1306FillRect(0, 58, 5, 5);
}
// else
// {
// ssd1306DrawRect(0,49,5,5);
// }
// ssd1306FillRect(0,0,5,5);
// ssd1306DrawRect(0,0,5,5);
if (fabs(getTelemeterSpeed(TELEMETER_DR)) > 500)
{
ssd1306FillRect(49, 58, 5, 5);
}
// else
// {
// ssd1306DrawRect(49,49,5,5);
// }
// ssd1306FillRect(49,49,5,5);
//
// ssd1306DrawRect(49,49,5,5);
//
// ssd1306FillRect(49,0,5,5);
//
// ssd1306DrawRect(49,0,5,5);
ssd1306PrintIntAtLine(55, 1, "FL", (int32_t) getTelemeterSpeed(TELEMETER_FL), &Font_5x8);
ssd1306PrintIntAtLine(55, 2, "FR", (int32_t) getTelemeterSpeed(TELEMETER_DL), &Font_5x8);
ssd1306PrintIntAtLine(55, 3, "DL", (int32_t) getTelemeterSpeed(TELEMETER_DR), &Font_5x8);
ssd1306PrintIntAtLine(55, 4, "DR", (int32_t) getTelemeterSpeed(TELEMETER_FR), &Font_5x8);
ssd1306Refresh();
}
telemetersStop();
}
void testWallsSensors()
{
telemetersInit();
telemetersStart();
while (expanderJoyFiltered() != JOY_LEFT)
{
ssd1306ClearScreen(MAIN_AREA);
if (getWallPresence(FRONT_WALL) == TRUE)
{
ssd1306FillRect(0, 59, 54, 5);
}
else
{
ssd1306DrawRect(0, 59, 54, 5);
}
if (getWallPresence(LEFT_WALL) == TRUE)
{
ssd1306FillRect(0, 10, 5, 54);
}
else
{
ssd1306DrawRect(0, 10, 5, 54);
}
if (getWallPresence(RIGHT_WALL) == TRUE)
{
ssd1306FillRect(49, 10, 5, 54);
}
else
{
ssd1306DrawRect(49, 10, 5, 54);
}
ssd1306PrintIntAtLine(60, 1, "FL ", (uint32_t) (getTelemeterDist(TELEMETER_FL) * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(60, 2, "DL ", (uint32_t) (getTelemeterDist(TELEMETER_DL) * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(60, 3, "DR ", (uint32_t) (getTelemeterDist(TELEMETER_DR) * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(60, 4, "FR ", (uint32_t) (getTelemeterDist(TELEMETER_FR) * 10.00), &Font_5x8);
ssd1306Refresh();
}
telemetersStop();
}
void mainControlDisplayTest(void)
{
while(expanderJoyFiltered()!=JOY_LEFT)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "speed dist = ",(int) (speed_control.current_distance * 100), &Font_5x8);
ssd1306PrintInt(10, 15, "follow err = ",(int) (wall_follow_control.follow_error), &Font_5x8);
ssd1306PrintInt(10, 25, "right_dist = ",(int) (position_control.end_control * 100), &Font_5x8);
ssd1306PrintInt(10, 35, "gyro = ",(int16_t) GyroGetAngle(), &Font_5x8);
ssd1306PrintInt(10, 45, "left PWM = ",(int16_t) transfert_function.left_motor_pwm, &Font_5x8);
ssd1306PrintInt(10, 55, "right PWM = ",(int16_t) transfert_function.right_motor_pwm, &Font_5x8);
// bluetoothPrintf("pwm right :%d \t %d \n",(int)transfert_function.right_motor_pwm, (int)(follow_control.follow_error*100));
// bluetoothPrintInt("error", follow_control.follow_error);
// transfert_function.right_motor_pwm = (speed_control.speed_command - (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
// transfert_function.left_motor_pwm = (speed_control.speed_command + (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
ssd1306Refresh();
}
pid_loop.start_state = FALSE;
telemetersStop();
motorsSleepDriver(ON);
}
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;
}
