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();
}
}
}
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 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 startDisplay(void) {
ssd1306Init(SSD1306_SWITCHCAPVCC);
ssd1306TurnOn();
display.state = DISPLAY_ON;
ssd1306DrawString(40, 20, "OpenQS", Font_System7x8);
chThdSleepMilliseconds(100); // Fails
serDbg("startDisplay Complete\r\n");
while (true) {
serDbg("Display OFF\r\n");
ssd1306TurnOff();
display.state = DISPLAY_OFF;
while (!BUTTON_SEL) /* Wait until the select button is pressed */
{
palTogglePad(GPIOC, GPIOC_LED4); /* Display heartbeat */
chThdSleepMilliseconds(250);
}
serDbg("Display ON\r\n");
ssd1306ClearScreen();
ssd1306TurnOn();
display.state = DISPLAY_ON;
openMenu(&mainMenu);
}
}
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 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();
}
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 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 setShifter(void)
{
drawTitle("Shifter Sensor Setup");
uint8_t i;
uint16_t peak;
ssd1306DrawString(0, 25, "Shift a gear", Font_System5x8);
ssd1306DrawString(10, 10, "to detect direction", Font_System5x8);
chThdSleepMilliseconds(2000);
peak = 0;
for (i=0; i<40; i++)
{
if (sensors.strain_gauge >= peak)
peak = sensors.strain_gauge;
chThdSleepMilliseconds(100);
}
ssd1306ClearScreen();
/* Peak >= 1.65v */
if (peak >= 0x8000)
{
ssd1306DrawString(0, 10, "Normal direction", Font_System5x8);
settings.data.sensor_direction = SETTINGS_SENSOR_NORMAL;
}
else
{
ssd1306DrawString(0, 5, "Reverse direction", Font_System5x8);
settings.data.sensor_direction = SETTINGS_SENSOR_REVERSE;
}
settings.data.sensor_threshold = peak;
writeSettings(&settings);
chThdSleepMilliseconds(2000);
while (!BUTTON_SEL) chThdSleepMilliseconds(100);
}
//Draws the actual menu
void drawMenu(menuStruct_t *menuToShow, short selectedIndex)
{
uint8_t i, y;
ssd1306ClearScreen();
ssd1306DrawString(0, 0, menuToShow->menuName, Font_System5x8);
for(i=0;i < ((menuToShow->numberItems+1) > 6 ? 6 : (menuToShow->numberItems+1)) ;i++)
{
y = (i*7)+1;
if (i > 5 + (selectedIndex > 5 ? selectedIndex - 5 : 0))
{
break;
}
else if (selectedIndex < 6)
{
if (selectedIndex == i) ssd1306DrawChar(0, y, '>', Font_System5x8);
if (selectedIndex > menuToShow->numberItems)
{
ssd1306DrawString(7, y, "Go Back", Font_System5x8);
}
else
{
ssd1306DrawString(7, y, menuToShow->items[i].itemName, Font_System5x8);
}
}
else
{
if (selectedIndex == i+(selectedIndex-5)) ssd1306DrawChar(0, y, '>', Font_System5x8);
if (selectedIndex > menuToShow->numberItems)
{
ssd1306DrawString(7, y, "Go Back", Font_System5x8);
}
else
{
ssd1306DrawString(7, y, menuToShow->items[i+(selectedIndex-5)].itemName, Font_System5x8);
}
}
}
return;
}
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 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 systemInit()
{
cpuInit();
systickInit((CFG_CPU_CCLK / 1000) * CFG_SYSTICK_DELAY_IN_MS);
gpioInit();
pmuInit();
adcInit(); // Init adc pins to avoid wasting 60uA in deep sleep
#ifdef CFG_PRINTF_UART
// Initialise UART with the default baud rate (set in projectconfig.h)
uartInit(CFG_UART_BAUDRATE);
#endif
// Switch to 3.3V if TPS780 (etc.) is being used
#if defined CFG_VREG_ALT_PRESENT && CFG_VREG_ALT_PRESENT == 1
gpioSetDir(CFG_VREG_ALT_PORT, CFG_VREG_ALT_PIN, gpioDirection_Output);
gpioSetValue(CFG_VREG_ALT_PORT, CFG_VREG_ALT_PIN, 0);
gpioSetPullup(&CFG_VREG_ALT_REG32, gpioPullupMode_Inactive);
#endif
// Set LED pin as output and turn LED off
gpioSetDir(CFG_LED_PORT, CFG_LED_PIN, 1);
gpioSetValue(CFG_LED_PORT, CFG_LED_PIN, CFG_LED_OFF);
// Initialise the ST7565 128x64 pixel display
#ifdef CFG_ST7565
st7565Init();
st7565ClearScreen(); // Clear the screen
st7565Backlight(1); // Enable the backlight
#endif
// Initialise the SSD1306 OLED display
#ifdef CFG_SSD1306
ssd1306Init(SSD1306_SWITCHCAPVCC);
ssd1306ClearScreen(); // Clear the screen
#endif
// Initialise EEPROM
#ifdef CFG_I2CEEPROM
mcp24aaInit();
#endif
// Initialise Chibi
#ifdef CFG_CHIBI
// Write addresses to EEPROM for the first time if necessary
// uint16_t addr_short = 0x1166;
// uint64_t addr_ieee = addr_short;
// mcp24aaWriteBuffer(CFG_CHIBI_EEPROM_SHORTADDR, (uint8_t *)&addr_short, 2);
// mcp24aaWriteBuffer(CFG_CHIBI_EEPROM_IEEEADDR, (uint8_t *)&addr_ieee, 8);
chb_init();
#endif
// Setup SD Card
#ifdef CFG_SDCARD
// Turn off SD card by default (saves power)
gpioSetDir(CFG_SDCARD_ENPORT, CFG_SDCARD_ENPIN, gpioDirection_Output); /* Set enable pin to output */
gpioSetValue(CFG_SDCARD_ENPORT, CFG_SDCARD_ENPIN, 0); /* Disable card by setting ENPIN low */
gpioSetPullup(&CFG_SDCARD_ENREG32, gpioPullupMode_Inactive);
#endif
#ifdef CFG_LM75B
// Initialise LM75B
lm75bInit();
// Read temp once to make sure we are in sleep mode
int32_t temp;
lm75bGetTemperature(&temp);
#endif
#ifdef CFG_BAT
// Turn off battery voltage divider by default
gpioSetDir(CFG_BAT_ENPORT, CFG_BAT_ENPIN, gpioDirection_Output ); // Set voltage divider enable pin to output
gpioSetValue(CFG_BAT_ENPORT, CFG_BAT_ENPIN, 0 ); // Disable the voltage divider by default
gpioSetPullup(&CFG_BAT_ENREG32, gpioPullupMode_Inactive);
#endif
// Start the command line interface (if requested)
#ifdef CFG_INTERFACE
printf("%sType '?' for a list of available commands%s", CFG_PRINTF_NEWLINE, CFG_PRINTF_NEWLINE);
cmdInit();
#endif
}
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 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;
}
// 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;
}
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;
}
