void testLine2 ()
{
char x=64,y=32;
while (1)
{
if (GPIO_ReadInputDataBit(JOYSTICK_LEFT) == Bit_RESET)
{
// Wait until button is released
hal_ui_anti_rebonds (JOYSTICK_LEFT);
y--;
x=(128-y)/-10;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306DrawPixel(x,y);
ssd1306DrawPixel(64,32);
ssd1306Refresh();
}
// Validate button
if(GPIO_ReadInputDataBit(JOYSTICK_RIGHT) == Bit_RESET)
{
hal_ui_anti_rebonds (JOYSTICK_RIGHT);
y++;
x=(128-y)/-10;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306Refresh();
}
}
}
int loadMaze(labyrinthe *maze, positionRobot *start_position, coordinate *end_coordinate)
{
MAZE_CONTAINER maze_container;
ssd1306ClearScreen(MAIN_AREA);
// Check if there is at least one stored maze in flash memory
if ((stored_mazes->count_stored_mazes < 0) || (stored_mazes->count_stored_mazes > MAX_STORABLE_MAZES))
{
ssd1306PrintfAtLine(0, 1, &Font_3x6, "Invalid stored mazes counter");
ssd1306Refresh();
return MAZE_SOLVER_E_ERROR;
}
// Get the first maze slot
memcpy(&maze_container, &(stored_mazes->mazes[0]), sizeof(MAZE_CONTAINER));
ssd1306PrintfAtLine(0, 1, &Font_3x6, "Maze restored successfully");
ssd1306Refresh();
memcpy(maze, &(maze_container.maze), sizeof(labyrinthe));
memcpy(start_position, &(maze_container.start_position), sizeof(positionRobot));
memcpy(end_coordinate, &(maze_container.end_coordinate), sizeof(coordinate));
#ifdef PRINT_BLUETOOTH_BASIC_DEGUG
bluetoothWaitReady();
bluetoothPrintf("start :\n\tcoordinates %d,%d\n\torientation %d\n\tmid of cell%d", start_position->coordinate_robot.x, start_position->coordinate_robot.y,start_position->orientation,start_position->midOfCell);
bluetoothWaitReady();
bluetoothPrintf("end :\n\tcoordinates %d,%d\n", end_coordinate->x, end_coordinate->y);
#endif
return MAZE_SOLVER_E_SUCCESS;
}
void test_Oled(void)
{
int i;
ExpanderSetbit(7,0);
HAL_Delay(100);
ExpanderSetbit(7,1);
HAL_Delay(100);
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306Refresh();
ssd1306DrawBmp(Pacabot_bmp, 1, 10, 128, 40);
ssd1306Refresh();
HAL_Delay(3500);
ssd1306ClearScreen();
ssd1306Refresh();
// miniature bitmap display
ssd1306DrawCircle(40, 30, 20);
// ssd1306DrawCircle(50, 20, 10);
ssd1306FillCircle(100, 40, 15);
ssd1306DrawRect(110, 3, 15, 6);
ssd1306FillRect(1, 60, 10, 20);
ssd1306DrawLine(5, 45, 70, 60);
ssd1306Refresh();
HAL_Delay(5500);
ssd1306ClearScreen();
for (i = 0; i <= 100; i+=2)
{
ssd1306ProgressBar(10, 20, i);
// HAL_Delay(1);
ssd1306Refresh();
}
ssd1306ShiftFrameBuffer(8);
ssd1306DrawString(13, 1, "Oled 128x64", &Font_8x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(1500);
ssd1306DrawString(1, 25, "Driver for STM32f4xx", &Font_5x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(500);
ssd1306DrawString(1, 35, "2 Wire SPI mode", &Font_5x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(1500);
ssd1306DrawString(10, 55, "BY PLF, PACABOT TEAM", &Font_3x6); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(5000);
ssd1306ClearScreen();
ssd1306Refresh();
}
void testLine ()
{
char x=64,y=32;
while (1)
{
if (GPIO_ReadInputDataBit(JOYSTICK_LEFT) == Bit_RESET)
{
// Wait until button is released
hal_ui_anti_rebonds (JOYSTICK_LEFT);
x--;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306DrawPixel(x,y);
ssd1306DrawPixel(64,32);
ssd1306Refresh();
}
// Joystick down
if (GPIO_ReadInputDataBit(JOYSTICK_DOWN) == Bit_RESET)
{
hal_ui_anti_rebonds (JOYSTICK_DOWN);
y++;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306DrawPixel(x,y);
ssd1306DrawPixel(64,32);
ssd1306Refresh();
}
// Joystick up
if (GPIO_ReadInputDataBit(JOYSTICK_UP) == Bit_RESET)
{
hal_ui_anti_rebonds (JOYSTICK_UP);
y--;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306DrawPixel(x,y);
ssd1306DrawPixel(64,32);
ssd1306Refresh();
}
// Validate button
if(GPIO_ReadInputDataBit(JOYSTICK_RIGHT) == Bit_RESET)
{
hal_ui_anti_rebonds (JOYSTICK_RIGHT);
x++;
ssd1306ClearScreen();
ssd1306DrawLine(64,32,x,y);
ssd1306DrawPixel(x,y);
ssd1306DrawPixel(64,32);
ssd1306Refresh();
}
}
}
void test_EasterEgg(void)
{
ExpanderSetbit(7,0);
HAL_Delay(100);
ExpanderSetbit(7,1);
HAL_Delay(100);
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306Refresh();
// I2C
uint8_t aTxBuffer[3]; // = {control, c};
aTxBuffer[0] = 0x0;
aTxBuffer[1] = 0x1;
aTxBuffer[2] = 0x5;
uint8_t aRxBuffer[1]; // = {control, c};
aRxBuffer[0] = 0x0;
ssd1306ClearScreen();
while(HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)0x50<<1, (uint8_t*)aTxBuffer, 3, 10000)!= HAL_OK)
{
/* Error_Handler() function is called when Timout error occurs.
When Acknowledge failure ocucurs (Slave don't acknowledge it's address)
Master restarts communication */
if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF)
{
ssd1306PrintInt(10, 15, "Bad Send", 0, &Font_5x8);
}
}
while(HAL_I2C_Master_Receive(&hi2c1, (uint16_t)0x50<<1, (uint8_t*)aRxBuffer, 1, 10000)!= HAL_OK)
{
/* Error_Handler() function is called when Timout error occurs.
When Acknowledge failure ocucurs (Slave don't acknowledge it's address)
Master restarts communication */
if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF)
{
ssd1306PrintInt(10, 25, "Bad recv", 0, &Font_5x8);
}
}
ssd1306PrintInt(10, 35, "Fini : ", aRxBuffer[0], &Font_5x8);
ssd1306Refresh();
}
int test_oled1(void)
{
int i;
ssd1306Init(0);
ssd1306Refresh();
ssd1306ClearScreen();
ssd1306DrawBmp(Pacabot_bmp, 1, 1, 128, 40);
ssd1306Refresh();
HAL_Delay(3500);
ssd1306ClearScreen();
ssd1306Refresh();
// miniature bitmap display
ssd1306DrawCircle(40, 30, 20);
// ssd1306DrawCircle(50, 20, 10);
ssd1306FillCircle(100, 40, 15);
ssd1306DrawRect(110, 3, 15, 6);
ssd1306FillRect(1, 60, 10, 20);
ssd1306DrawLine(5, 45, 70, 60);
ssd1306Refresh();
HAL_Delay(5500);
ssd1306ClearScreen();
for (i = 0; i <= 100; i++)
{
ssd1306ProgressBar(10, 20, i);
HAL_Delay(1);
ssd1306Refresh();
}
ssd1306ShiftFrameBuffer(8);
ssd1306DrawString(13, 1, "Oled 128x64", &Font_8x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(1500);
ssd1306DrawString(1, 25, "Driver for STM32f10x", &Font_5x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(500);
ssd1306DrawString(1, 35, "4 Wire SPI mode", &Font_5x8); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(1500);
ssd1306DrawString(10, 55, "BY PLF ALL RIGHTS RESERVED", &Font_3x6); // 3x6 is UPPER CASE only
ssd1306Refresh();
HAL_Delay(5000);
ssd1306ClearScreen();
ssd1306Refresh();
return 0;
}
int test_oled2(void)
{
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306DrawBmp(pic_battery_level, 120, 0, 8, 8);
ssd1306Refresh();
HAL_Delay(4000);
return 0;
}
void mulimeterTest(void)
{
while (expanderJoyFiltered() != JOY_LEFT)
{
ssd1306ClearScreen(MAIN_AREA);
ssd1306PrintIntAtLine(0, 0, "Temp. Gyro = ", (int) (multimeterGyroTemp() * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(0, 1, "Temp. STM32 = ", (int) (multimeterSTM32Temp() * 10.00), &Font_5x8);
ssd1306PrintIntAtLine(0, 3, "vbat (mV) = ", (int) multimeterGetBatVoltage(), &Font_5x8);
ssd1306Refresh();
}
}
void bluetoothTest(void)
{
int i = 0;
while (expanderJoyFiltered() != JOY_LEFT)
{
bluetoothPrintf("hello ZHONX_III, nb send = %d\r\n", i);
ssd1306ClearScreen(MAIN_AREA);
ssd1306DrawStringAtLine(0, 0, "send hello ZHONX III", &Font_5x8);
ssd1306PrintIntAtLine(0, 1, "nb send = ", i, &Font_5x8);
ssd1306Refresh();
i++;
}
antiBounceJoystick();
}
void test_Telemeters(void)
{
Telemeters_Init();
ssd1306Init(0);
while(1)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "LFRONT = ", (int) L_front_distance, &Font_5x8);
ssd1306PrintInt(10, 15, "RFRONT = ", (int) R_front_distance, &Font_5x8);
ssd1306PrintInt(10, 25, "LDIAG = ", (int) L_diag_distance, &Font_5x8);
ssd1306PrintInt(10, 35, "RDIAG = ", (int) R_diag_distance, &Font_5x8);
ssd1306Refresh();
}
}
void joystickTest(void)
{
int state;
while(expanderJoyState()!=JOY_LEFT)
{
state = expanderJoyState();
ssd1306ClearScreen();
ssd1306DrawCircle(60,10, 3);
ssd1306DrawCircle(60,30, 3);
ssd1306DrawCircle(50,20, 3);
ssd1306DrawCircle(70,20, 3);
switch (state)
{
case JOY_UP:
ssd1306FillCircle(60,10, 3);
break;
case JOY_DOWN:
ssd1306FillCircle(60,30, 3);
break;
case JOY_LEFT:
ssd1306FillCircle(50,20, 3);
break;
case JOY_RIGHT:
ssd1306FillCircle(70,20, 3);
break;
}
ssd1306Refresh();
}
ssd1306FillCircle(50,20, 3);
ssd1306Refresh();
HAL_Delay(1000);
antiBounceJoystick();
}
void test_Encoders(void)
{
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306Refresh();
Encoders_Init();
while(1)
{
ssd1306ClearScreen();
// ssd1306PrintInt(0, 6, "REV = ", toto, &Font_3x6);
// ssd1306PrintInt(0, 13, "CNT = ", (&htim1)->Instance->CNT, &Font_3x6);
ssd1306PrintInt(0, 7, "L_REV = ", left_encoder.nb_revolutions, &Font_3x6);
ssd1306PrintInt(0, 14, "L_CNT = ", __HAL_TIM_GetCounter(&htim1), &Font_3x6);
ssd1306PrintInt(0, 21, "L_DIR = ", __HAL_TIM_DIRECTION_STATUS(&htim1), &Font_3x6);
ssd1306PrintInt(0, 35, "R_REV = ", right_encoder.nb_revolutions, &Font_3x6);
ssd1306PrintInt(0, 42, "R_CNT = ", __HAL_TIM_GetCounter(&htim3), &Font_3x6);
ssd1306PrintInt(0, 49, "R_DIR = ", __HAL_TIM_DIRECTION_STATUS(&htim3), &Font_3x6);
ssd1306Refresh();
HAL_Delay(10);
}
}
void test_Vbat(void)
{
ADC_ChannelConfTypeDef sConfig;
int vbat;
int vref = 3300; //mV
int coeff = 33; //%
ExpanderSetbit(7,0);
HAL_Delay(100);
ExpanderSetbit(7,1);
HAL_Delay(100);
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306Refresh();
while(1)
{
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, SET);
ssd1306ClearScreen();
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_15;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_56CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_8, 1);
vbat = (((vref * uhADCxConvertedValue_8) / 40950) * coeff);
ssd1306PrintInt(10, 5, "Vbat = ", vbat, &Font_5x8);
ssd1306Refresh();
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, RESET);
HAL_Delay(100);
}
}
void bluetoothInit(void)
{
ssd1306ClearScreen(MAIN_AREA);
ssd1306Printf(12, 30, &Font_3x6, "%s", "Initializing Bluetooth...");
ssd1306Refresh();
// Allow Remote Escape Sequence
bluetoothCmd("AT+AB Config RmtEscapeSequence = true");
// Reset the bluetooth peripheral
bluetoothCmd("AT+AB Reset");
// Enable USART Interrupts
bluetoothEnable();
}
walls ask_cell_state ()
{
walls cell_state;
memset(&cell_state, NO_KNOWN, sizeof(walls));
int joystick = expanderJoyFiltered();
while (joystick != JOY_UP)
{
joystick = expanderJoyFiltered();
switch (joystick) {
case JOY_DOWN:
if (cell_state.front == WALL_PRESENCE)
{
cell_state.front = NO_WALL;
}
else
{
cell_state.front = WALL_PRESENCE;
}
break;
case JOY_RIGHT:
if (cell_state.left == WALL_PRESENCE)
{
cell_state.left = NO_WALL;
}
else
{
cell_state.left = WALL_PRESENCE;
}
break;
case JOY_LEFT:
if (cell_state.right == WALL_PRESENCE)
{
cell_state.right = NO_WALL;
}
else
{
cell_state.right = WALL_PRESENCE;
}
break;
default:
break;
}
print_cell_state(cell_state);
ssd1306Refresh();
}
return cell_state;
}
void displayMenu(const menuItem menu,int line)
{
//char str[5];
ssd1306ClearScreen();
ssd1306DrawString(0,0,menu.name,&Font_5x8);
ssd1306DrawLine(0,8,128,8);
for (int i=0;i<MAX_LINE_SCREEN;i++)
{
if(menu.line[i].name!=null)
ssd1306DrawString(0,MARGIN*i+MARGIN+1,menu.line[line+i].name,&Font_5x8);
switch (menu.line[line+i].type)
{
case 'b':
if(*((bool*)menu.line[i+line].param)==true)
ssd1306DrawString(90,MARGIN*i+MARGIN+1,"yes",&Font_5x8);
else
ssd1306DrawString(90,MARGIN*i+MARGIN+1,"no",&Font_5x8);
break;
case 'i':
ssd1306PrintInt(90,MARGIN*i+MARGIN+1," ",*((unsigned int*)menu.line[i+line].param),&Font_3x6);
break;
case 'l':
ssd1306PrintInt(90,MARGIN*i+MARGIN+1," ",*((unsigned long*)menu.line[i+line].param),&Font_3x6);
break;
case 'f':
ssd1306DrawString(110,i*MARGIN+MARGIN+1,"->",&Font_3x6);
break;
case 'm':
ssd1306DrawString(115,i*MARGIN+MARGIN+1,">",&Font_3x6);
break;
// case 'a':
// sprintf(str,"%f.2",*(float*)menu.line[i+line].param);
// ssd1306DrawString(110,i*MARGIN+MARGIN+1,str,&Font_3x6);
}
}
uint8_t nmbr_item = 0;
while(menu.line[nmbr_item].name != null)
{
nmbr_item++;
}
if (nmbr_item>MAX_LINE_SCREEN)
{
//int heightOneItem=54/nmbr_item;
ssd1306DrawRect(123,(54*line)/nmbr_item+MARGIN,3,(54*MAX_LINE_SCREEN)/nmbr_item);
}
ssd1306Refresh();
}
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 lineSensorsTest(void)
{
lineSensorsInit();
lineSensorsStart();
while(expanderJoyFiltered()!=JOY_LEFT)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "LEFT_EXT = ", (uint16_t) lineSensors.left_ext.adc_value, &Font_5x8);
ssd1306PrintInt(10, 15, "LEFT = ", (uint16_t) lineSensors.left.adc_value, &Font_5x8);
ssd1306PrintInt(10, 25, "FRONT = ", (uint16_t) lineSensors.front.adc_value, &Font_5x8);
ssd1306PrintInt(10, 35, "RIGHT = ", (uint16_t) lineSensors.right.adc_value, &Font_5x8);
ssd1306PrintInt(10, 45, "RIGHT_EXT = ", (uint16_t) lineSensors.right_ext.adc_value, &Font_5x8);
ssd1306Refresh();
}
lineSensorsStop();
}
void test_Gyro(void)
{
ADXRS620_Init();
ssd1306Init(0);
while(1)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 25, "Angle = ", (int) gyro_Current_Angle, &Font_5x8);
int y = 0;
for(int i = 0; i<100; i++)
{
y += uhADCxConvertedValue_Gyro, &Font_5x8;
HAL_Delay(1);
}
ssd1306PrintInt(10, 35, "Adc = ", ((y/100)*3300)/4095, &Font_5x8);
ssd1306Refresh();
}
}
int test_oled3(void)
{
//int i;
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306DrawBmp(cafard, 12, 0, 103, 64);
ssd1306Refresh();
// for (i = 0; i < 10; i++)
// {
// hal_os_sleep(500);
// ssd1306InvertArea(39, 39, 10, 10);
// ssd1306Refresh();
// }
HAL_Delay(10000);
return 0;
}
void printGraphMotor (float acceleration, float maxSpeed, float deceleration)
{
char str[10];
ssd1306ClearScreen();
char point1[2]={(char)((0-maxSpeed)/(0-acceleration)),64-(char)maxSpeed};
char point2[2]={(char)(128-(0-maxSpeed)/(0-deceleration)),64-(char)(maxSpeed)};
ssd1306DrawLine(0,64,point1[0],point1[1]);
ssd1306DrawLine(point1[0],point1[1],point2[0],point2[1]);
ssd1306DrawLine(point2[0],point1[1],128,64);
sprintf(str,"%.2fM.S^2",acceleration);
ssd1306DrawString((0+point1[0])/2+2,(64+point1[1])/2+2,str,&Font_3x6);
sprintf(str,"%.2fM.S",maxSpeed);
ssd1306DrawString((point1[0]+point2[0])/2,(point1[1]+point2[1])/2,str,&Font_3x6);
sprintf(str,"%.2fM.S^2",deceleration);
ssd1306DrawString((point2[0]+128)/2-27,(point2[1]+64)/2,str,&Font_3x6);
ssd1306Refresh();
}
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);
}
void menuHighlightedMove(unsigned char y, unsigned char max_y)
{
if (max_y > y)
{
// ssd1306InvertArea(0, y-1, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT*2);
for ( ; y <= max_y; y++)
{
ssd1306InvertArea(0, y - 1, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306InvertArea(0, y, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
// ssd1306Refresh();
}
}
else
{
// ssd1306InvertArea(0, y-HIGHLIGHT_HEIGHT+1, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT*2);
for ( ; y >= max_y; y--)
{
ssd1306InvertArea(0, y + 1, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306InvertArea(0, y, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
// ssd1306Refresh();
}
}
ssd1306Refresh();
}
int modifyLongParam( char *param_name,long *param)
{
int step=1;
char str[40];
long param_copy = *param;
char collone=0;
ssd1306ClearScreen();
// Write the parameter name
ssd1306DrawString(0, 0,param_name, &Font_5x8);
ssd1306DrawLine(0, 9, 128, 9);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306DrawString(0, 50, "PRESS 'RIGHT' TO VALIDATE", &Font_3x6);
ssd1306DrawString(0, 57, " 'LEFT' TO RETURN.", &Font_3x6);
ssd1306DrawString((10-collone)*8,20,"^",&Font_8x8);
ssd1306DrawString((10-collone)*8,36,"v",&Font_8x8);
ssd1306Refresh();
while (1)
{
// Exit Button
int joystick=expanderJoyFiltered();
switch (joystick)
{
case JOY_LEFT :
if (collone==10)
return SUCCESS;
else
{
collone++;
ssd1306ClearRect(0,20,128,8);
ssd1306ClearRect(0,36,128,8);
ssd1306DrawString((9-collone)*9,20,"^",&Font_8x8);
ssd1306DrawString((9-collone)*9,36,"v",&Font_8x8);
ssd1306Refresh();
}
break;
case JOY_UP:
//param_copy +=1;
param_copy += (step*pow(10,collone));
ssd1306ClearRect(0, 28, 164, 8);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_DOWN :
param_copy -= (step*pow(10,collone));
//param_copy -= 1;
ssd1306ClearRect(0, 28, 164, 8);
sprintf(str, "%10i", (int)param_copy);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_RIGHT :
if(collone==0)
{
*param = param_copy;
ssd1306Refresh();
return SUCCESS;
}
else
{
collone--;
ssd1306ClearRect(0,20,128,8);
ssd1306ClearRect(0,36,128,8);
ssd1306DrawString((9-collone)*9,20,"^",&Font_8x8);
ssd1306DrawString((9-collone)*9,36,"v",&Font_8x8);
ssd1306Refresh();
}
break;
default:
break;
}
}
return SUCCESS;
}
int modifyBoolParam( char *param_name, unsigned char *param)
{
char str[4];
bool param_copy = (bool)*param;
ssd1306ClearScreen();
// Write the parameter name
ssd1306DrawString(0, 0,param_name, &Font_5x8);
ssd1306DrawLine(0, 9, 128, 9);
if (param_copy == true)
{
sprintf(str, "YES");
}
else
{
sprintf(str, "NO");
}
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306DrawString(0, 50, "PRESS 'RIGHT' TO VALIDATE", &Font_3x6);
ssd1306DrawString(0, 57, " 'LEFT' TO RETURN.", &Font_3x6);
ssd1306Refresh();
while (1)
{
int joystick=expanderJoyFiltered();
switch (joystick)
{
case JOY_LEFT :
return SUCCESS;
break;
case JOY_DOWN:
case JOY_UP :
if (param_copy == true)
{
param_copy = false;
sprintf(str, "NO");
}
else
{
param_copy = true;
sprintf(str, "YES");
}
ssd1306ClearRect(0, 28, 164, 8);
ssd1306DrawString(0, 28, str, &Font_8x8);
ssd1306Refresh();
break;
case JOY_RIGHT:
*param = param_copy;
ssd1306ClearScreen();
ssd1306Refresh();
return SUCCESS;
break;
}
}
return SUCCESS;
}
int menu(const menuItem Menu)
{
signed char line_screen = 1;
signed char line_menu = 0;
displayMenu(Menu, line_menu);
ssd1306InvertArea(0, MARGIN, HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306Refresh();
while (true)
{
int joystick = expanderJoyFiltered();
// Exit Button JOYSTICK_LEFT
switch (joystick)
{
case JOY_LEFT:
return SUCCESS;
break;
// Joystick down
case JOY_DOWN:
//beeper
if (Menu.line[line_menu + 1].name != null)
{
line_menu++;
line_screen++;
if (line_screen > MAX_LINE_SCREEN)
{
line_screen--;
displayMenu(Menu, line_menu - (line_screen - 1));
ssd1306InvertArea(0, line_screen * MARGIN,
HIGHLIGHT_LENGHT, HIGHLIGHT_HEIGHT);
ssd1306Refresh();
}
else
{
menuHighlightedMove((line_screen - 1) * ROW_HEIGHT + 1,
(line_screen) * ROW_HEIGHT);
}
}
break;
case JOY_UP:
//beeper
if (line_screen == 1)
{
if (line_menu > 0)
{
line_menu--;
displayMenu(Menu, line_menu);
ssd1306InvertArea(0, MARGIN, HIGHLIGHT_LENGHT,
HIGHLIGHT_HEIGHT);
ssd1306Refresh();
}
}
else
{
line_menu--;
line_screen--;
menuHighlightedMove((line_screen + 1) * ROW_HEIGHT - 1,
(line_screen) * ROW_HEIGHT);
}
break;
case JOY_RIGHT: // Validate button joystick right
//hal_beeper_beep(app_context.beeper, 4000, 10);
switch (Menu.line[line_menu].type)
{
case 'b':
modifyBoolParam(Menu.line[line_menu].name,
(unsigned char*) Menu.line[line_menu].param);
break;
case 'i':
modifyLongParam(Menu.line[line_menu].name,
(long*) (int*) Menu.line[line_menu].param);
break;
case 'l':
modifyLongParam(Menu.line[line_menu].name,
(long*) Menu.line[line_menu].param);
break;
case 'm':
menu(*(const menuItem*) Menu.line[line_menu].param);
break;
case 'f':
if (Menu.line[line_menu].param != null)
{
ssd1306ClearScreen();
ssd1306Refresh();
Menu.line[line_menu].param();
}
break;
case 'g':
graphMotorSettings(
(float*) Menu.line[line_menu - 3].param,
(float*) Menu.line[line_menu - 2].param,
(float*) Menu.line[line_menu - 1].param);
break;
default:
break;
}
displayMenu(Menu, line_menu - (line_screen - 1));
ssd1306InvertArea(0, MARGIN * line_screen, HIGHLIGHT_LENGHT,
HIGHLIGHT_HEIGHT);
ssd1306Refresh();
break;
default:
break;
}
cmdline_parse();
}
return -1;
}
void test_LineSensors(void)
{
ADC_ChannelConfTypeDef sConfig;
ExpanderSetbit(7,0);
HAL_Delay(100);
ExpanderSetbit(7,1);
HAL_Delay(100);
ssd1306Init(0);
ssd1306ClearScreen();
ssd1306Refresh();
while(1)
{
ssd1306ClearScreen();
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, SET);
HAL_Delay(25);
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_3;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_1, 1);
ssd1306PrintInt(10, 5, "ADC3 = ", uhADCxConvertedValue_1, &Font_5x8);
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_4;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_2, 1);
ssd1306PrintInt(10, 15, "ADC4 = ", uhADCxConvertedValue_2, &Font_5x8);
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_1;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_3, 1);
ssd1306PrintInt(10, 25, "ADC1 = ", uhADCxConvertedValue_3, &Font_5x8);
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_13;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_4, 1);
ssd1306PrintInt(10, 35, "ADC13 = ", uhADCxConvertedValue_4, &Font_5x8);
HAL_ADC_Stop_DMA(&hadc1);
sConfig.Channel = ADC_CHANNEL_12;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&uhADCxConvertedValue_5, 1);
ssd1306PrintInt(10, 45, "ADC12 = ", uhADCxConvertedValue_5, &Font_5x8);
ssd1306Refresh();
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, RESET);
}
}
// Save a maze into the flash memory
int saveMaze(labyrinthe *maze, positionRobot *start_position, coordinate *end_coordinate)
{
MAZE_CONTAINER maze_container;
int rv = 0;
int selected_maze = 0;
int cnt_mazes = stored_mazes->count_stored_mazes;
memcpy(&(maze_container.maze), maze, sizeof(labyrinthe));
memcpy(&(maze_container.start_position), start_position, sizeof(positionRobot));
memcpy(&(maze_container.end_coordinate), end_coordinate, sizeof(coordinate));
#if 0
// Check whether flash data have been initialized
if ((cnt_mazes > 0) &&
(cnt_mazes < MAX_STORABLE_MAZES))
{
// TODO: Display a menu to select a maze slot in flash memory
}
else
{
#endif
ssd1306ClearScreen(MAIN_AREA);
// Store maze in the first slot
// Initialize maze counter
ssd1306PrintfAtLine(0, 1, &Font_3x6, "Saving maze counter...");
ssd1306Refresh();
cnt_mazes = 1;
rv = flash_write(zhonxSettings.h_flash, (unsigned char *)&(stored_mazes->count_stored_mazes),
(unsigned char *)&cnt_mazes, sizeof(int));
if (rv != FLASH_DRIVER_E_SUCCESS)
{
ssd1306PrintfAtLine(0, 2, &Font_3x6, "Failed to save maze counter!");
ssd1306Refresh();
return rv;
}
ssd1306PrintfAtLine(0, 2, &Font_3x6, "Maze counter save successfully");
ssd1306Refresh();
// Store maze
ssd1306PrintfAtLine(0, 3, &Font_3x6, "Saving maze...");
ssd1306Refresh();
rv = flash_write(zhonxSettings.h_flash, (unsigned char *)&(stored_mazes->mazes[selected_maze].maze),
(unsigned char *)&maze_container, sizeof(MAZE_CONTAINER));
if (rv != FLASH_DRIVER_E_SUCCESS)
{
ssd1306PrintfAtLine(0, 4, &Font_3x6, "Failed to save maze container!");
ssd1306Refresh();
bluetoothPrintf("Failed to save maze container!");
return rv;
}
// Verify written data in flash memory
rv = memcmp(maze, &(stored_mazes->mazes[selected_maze].maze), sizeof(labyrinthe));
if (rv != 0)
{
ssd1306PrintfAtLine(0, 4, &Font_3x6, "Failed to save maze!");
ssd1306Refresh();
bluetoothPrintf("Failed to save maze!");
return rv;
}
rv = memcmp(start_position, &(stored_mazes->mazes[selected_maze].start_position), sizeof(positionRobot));
if (rv != 0)
{
ssd1306PrintfAtLine(0, 4, &Font_3x6, "Failed to save start position!");
ssd1306Refresh();
bluetoothPrintf("Failed to save maze start position!");
return rv;
}
rv = memcmp(end_coordinate, &(stored_mazes->mazes[selected_maze].end_coordinate), sizeof(coordinate));
if (rv != 0)
{
ssd1306PrintfAtLine(0, 4, &Font_3x6, "Failed to save end coordinate!");
ssd1306Refresh();
bluetoothPrintf("Failed to save maze start end coordinate!");
return rv;
}
ssd1306PrintfAtLine(0, 4, &Font_3x6, "Maze saved successfully");
ssd1306Refresh();
#if 0
}
#endif
#ifdef PRINT_BLUETOOTH_BASIC_DEGUG
bluetoothWaitReady();
bluetoothPrintf("start :\n\tcoordinates %d,%d\n\torientation %d\n\tmid of cell%d", start_position->coordinate_robot.x, start_position->coordinate_robot.y,start_position->orientation,start_position->midOfCell);
bluetoothWaitReady();
bluetoothPrintf("end :\n\tcoordinates %d,%d\n", end_coordinate->x, end_coordinate->y);
#endif
return 0;
}
void test_Motor_Move() {
GPIO_InitTypeDef GPIO_InitStruct;
TIM_ClockConfigTypeDef sClockSourceConfig;
TIM_OC_InitTypeDef sConfigOC;
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
TIM_MasterConfigTypeDef sMasterConfig;
ADXRS620_Init();
init_display();
uint32_t coef = 6;
uint32_t correct = 0;
long K1 = 1571;
long K2 = 1532;
long K3 = 225;
// 1571
// 1532
// 225
// 23562
// 22982
// 225
int PWMOld = 0;
int PWM =0;
int error =0;
int errorOld=0;
int consigne = 0;
htim8.Instance = TIM8;
htim8.Init.Prescaler = 4;
htim8.Init.CounterMode = TIM_COUNTERMODE_UP;
htim8.Init.Period = 1000-1;
htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim8.Init.RepetitionCounter = 0;
HAL_TIM_Base_Init(&htim8);
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
HAL_TIM_ConfigClockSource(&htim8, &sClockSourceConfig);
HAL_TIM_PWM_Init(&htim8);
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 500;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCPOLARITY_LOW;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1);
// HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_2);
// HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_3);
HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_4);
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig);
GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, SET);
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, SET);
uint32_t Pulses[2] = {50,50};
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, RESET);
// sConfigOC.OCMode = TIM_OCMODE_PWM1;
// sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
// sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, SET);
//for(int y = 0; y < 4000; y++)
consigne = 200;
PWM = consigne;
// PWM_R = consigne;
while(1)
{
PWMOld = PWM;
errorOld = error;
error = (int32_t) gyro_Current_Angle;
PWM = (K1*error - K2*errorOld + K3*PWMOld)/256;
if ((((consigne - PWM) + consigne) > 0) && (((PWM - consigne) + consigne) > 0))
{
Pulses[0] = (consigne - PWM) + consigne;
Pulses[1] = (PWM - consigne) + consigne;
}
// HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
// HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_4);
sConfigOC.Pulse = Pulses[0];
HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1);
sConfigOC.Pulse = (1000 - Pulses[1]);
HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_4);
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4);
HAL_Delay(1);
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "Correct = ", error, &Font_5x8);
ssd1306PrintInt(10, 15, "Pulses[0] = ", Pulses[0], &Font_5x8);
ssd1306PrintInt(10, 25, "Pulses[1] = ", Pulses[1], &Font_5x8);
ssd1306Refresh();
}
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, RESET);
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, RESET);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, RESET);
HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_4);
// HAL_Delay(1);
}
