/* * @Author: Wang Feixuan * @Email: cnfxwang@gmail.com * @Last Modified time: 2016-08-25 11:13:48 */ #include "eyebot.h" // for lcd area #define HOLLOW 0 #define FILLED 1 // for key codes /** * KEY1 1 * KEY2 2 * KEY3 4 * KEY4 8 * NOKEY 0 * ANYKEY 0xffffffff */ #define LINE_SPEED_DEC 11 #define LINE_SPEED_INC 12 #define ANG_SPEED_DEC 13 #define ANG_SPEED_INC 14 #define LINE_DIST_DEC 15 #define LINE_DIST_INC 16 #define CURVE_DIST_DEC 17 #define CURVE_DIST_INC 18 #define TURN_ANG_DEC 19 #define TURN_ANG_INC 20 #define TURN_REVERSE 22 #define VV_DEC 27 #define VV_INC 28 #define TV_DEC 29 #define TV_INC 30 #define VW_DEC 31 #define VW_INC 32 #define TW_DEC 33 #define TW_INC 34 #define STEP_DEC 37 #define STEP_INC 38 // for motors int lin_speed = 50; int ang_speed = 50; int lin_dist = 20; int curve_dist = 20; int turn_ang = 90; // step added every time int incr_step = 1; // for PID controllers int Vv = 75; int Tv = 45; int Vw = 65; int Tw = 10; // for LCDSetPrintf() #define x1 10 #define x2 125 #define x3 345 #define x4 460 #define col1 0 #define col2 30 #define col3 12 #define step 20 // for KEY_Decode #define LEFT 1 #define RIGHT 2 #define MIDDLE 3 int side_x(int x) { if (x1 < x && x < x2) return LEFT; if (x3 < x && x < x4) return RIGHT; return MIDDLE; } // there's already a private function called KEYDecode() in eyebot.h int KEY_Decode(int x, int y) { // KEY1 to KEY4 if (y > 285) { if (x < 120) return KEY1; if (x < 240) return KEY2; if (x < 360) return KEY3; return KEY4; } int col = side_x(x); int row = y / step; if (col == MIDDLE) { if (row == 4) return TURN_REVERSE; return NOKEY; } if (row == 5 || row == 6 || row == 7 || row == 12 || row > 13) return NOKEY; return 10 + col + row * 2; } // reprint the parameter information every time something is changed // also redraw triangles as somehow the colors can be changed otherwise void lcd_print_info() { int i = 0; for (; i < 15; i++) { if (i == 6 || i == 7 || i == 8 || i == 13) // blank lines continue; LCDArea(x1, step * (i - 1), x2, step * i, GREEN, HOLLOW); LCDArea(x3, step * (i - 1), x4, step * i, RED, HOLLOW); } LCDSetFontSize(14); LCDSetFont(COURIER, BOLD); LCDSetColor(WHITE, BLACK); LCDSetPrintf(0, col3, "line speed = %3d", lin_speed); LCDSetPrintf(1, col3, "angle speed = %3d", ang_speed); LCDSetPrintf(2, col3, "line dist = %3d", lin_dist); LCDSetPrintf(3, col3, "curve dist = %3d", curve_dist); LCDSetPrintf(4, col3, "turn angle = %3d", turn_ang); LCDSetPrintf(6, col3, "For PID controllers:"); LCDSetPrintf(8, col3, "(straight)Vv =%3d", Vv); LCDSetPrintf(9, col3, "(straight)Tv =%3d", Tv); LCDSetPrintf(10, col3, "(turning) Vw =%3d", Vw); LCDSetPrintf(11, col3, "(turning) Tw =%3d", Tw); LCDSetPrintf(13, col3, "incr step = %3d", incr_step); // turn reverse(clockwise / anti-clockwise) i = 5; LCDArea(x2 + 15, step * (i - 1), x3 - 15, step * i, YELLOW, HOLLOW); } // "+""-" on the buttons can be drawn once only void draw_keys() { LCDSetFontSize(14); LCDSetFont(COURIER, BOLD); LCDSetColor(WHITE, GREEN); LCDSetPrintf(0, col1, " - "); LCDSetPrintf(1, col1, " - "); LCDSetPrintf(2, col1, " - "); LCDSetPrintf(3, col1, " - "); LCDSetPrintf(4, col1, " - "); LCDSetPrintf(8, col1, " - "); LCDSetPrintf(9, col1, " - "); LCDSetPrintf(10, col1, " - "); LCDSetPrintf(11, col1, " - "); LCDSetPrintf(13, col1, " - "); LCDSetColor(WHITE, RED); LCDSetPrintf(0, col2, " + "); LCDSetPrintf(1, col2, " + "); LCDSetPrintf(2, col2, " + "); LCDSetPrintf(3, col2, " + "); LCDSetPrintf(4, col2, " + "); LCDSetPrintf(8, col2, " + "); LCDSetPrintf(9, col2, " + "); LCDSetPrintf(10, col2, " + "); LCDSetPrintf(11, col2, " + "); LCDSetPrintf(13, col2, " + "); } void VWStop() { VWStraight(0, 0); VWWait(); } time_t rawtime; struct tm *start_time, *end_time; /** * @brief save the all the parameters to a file for next time */ void VWExit() { VWStop(); LCDClear(); LCDPrintf("\n\nFinal parameters:\n"); LCDPrintf("(straight)Vv = %3d\n", Vv); LCDPrintf("(straight)Tv= %3d\n", Tv); LCDPrintf("(turning) Vw = %3d\n", Vw); LCDPrintf("(turning) Tw = %3d\n", Tw); LCDPrintf("\n\nExiting the program..\n"); sleep(2); // writing to the log file stdout = freopen("/home/pi/usr/software/control/PID_tuning.log", "a+", stdout); time(&rawtime); end_time = localtime(&rawtime); printf("*********************************\n"); printf("from %s", asctime(start_time)); printf("to %s", asctime(end_time)); printf("\n"); printf("Final parameters\n"); printf("(straight proportional) Vv = %3d\n", Vv); printf("(straight integral ) Tv= %3d\n", Tv); printf("(turning proportioanl) Vw = %3d\n", Vw); printf("(turning integral ) Tw = %3d\n", Tw); printf("*********************************\n\n\n"); // save final parameters for next time stdout = freopen("/home/pi/usr/software/control/PID_tuning.parameters.txt", "w", stdout); printf("%d\n", lin_speed); printf("%d\n", ang_speed); printf("%d\n", lin_dist); printf("%d\n", curve_dist); printf("%d\n", turn_ang); printf("%d\n", Vv); printf("%d\n", Tv); printf("%d\n", Vw); printf("%d\n", Tw); printf("%d\n", incr_step); exit(-1); } int main() { time(&rawtime); start_time = localtime(&rawtime); // read parameters from last test // if no parameters provided, use default if ((stdin = freopen("/home/pi/usr/software/control/PID_tuning.parameters.txt", "r", stdin))) { scanf("%d\n", &lin_speed); scanf("%d\n", &ang_speed); scanf("%d\n", &lin_dist); scanf("%d\n", &curve_dist); scanf("%d\n", &turn_ang); scanf("%d\n", &Vv); scanf("%d\n", &Tv); scanf("%d\n", &Vw); scanf("%d\n", &Tw); scanf("%d\n", &incr_step); } LCDMenu("STRAIGHT", "TURN", "CURVE", "EXIT"); LCDSetFontSize(14); LCDSetFont(COURIER, BOLD); draw_keys(); lcd_print_info(); VWControl(Vv, Tv, Vw, Tw); int x = 0, y = 0; KEYGetXY(&x, &y); int key_code = KEY_Decode(x, y); while (true) { switch (key_code) { case KEY1: VWStraight(lin_dist * 10, lin_speed); // VWWait(); // usleep(500); // VWSetSpeed(0, 0); // VWSetPosition(0, 0, 0); break; case KEY2: VWTurn((int) (turn_ang * 31.4 / 1.8), ang_speed); // VWWait(); // usleep(500); // VWSetSpeed(0, 0); // VWSetPosition(0, 0, 0); break; case KEY3: VWCurve(curve_dist * 10, (int) (turn_ang * 3.14 / 1.8), lin_speed); // VWWait(); // usleep(500); // VWSetSpeed(0, 0); // VWSetPosition(0, 0, 0); break; case KEY4: VWExit(); break; case LINE_SPEED_DEC: if (lin_speed - incr_step >= 0) lin_speed -= incr_step; break; // max safe speed case LINE_SPEED_INC: lin_speed += incr_step; break; case ANG_SPEED_DEC: if (ang_speed - incr_step >= 0) ang_speed -= incr_step; break; case ANG_SPEED_INC: ang_speed += incr_step; break; case LINE_DIST_DEC: if (lin_dist - incr_step >= 0) lin_dist -= incr_step; break; case LINE_DIST_INC: lin_dist += incr_step; break; case CURVE_DIST_DEC: if (curve_dist - incr_step >= 0) curve_dist -= incr_step; break; case CURVE_DIST_INC: curve_dist += incr_step; break; // angles can be either above and below zero // for anti-clockwise and clock-wise case TURN_ANG_DEC: if (turn_ang >= 0) { if (turn_ang - incr_step >= 0) turn_ang -= incr_step; } else { if (turn_ang + incr_step <= 0) turn_ang += incr_step; } break; case TURN_ANG_INC: if (turn_ang >= 0) { turn_ang += incr_step; } else { turn_ang -= incr_step; } break; case TURN_REVERSE: turn_ang *= -1; break; case VV_DEC: if (Vv - incr_step >= 0) Vv -= incr_step; break; case VV_INC: Vv += incr_step; break; case TV_DEC: if (Tv - incr_step >= 0) Tv -= incr_step; break; case TV_INC: Tv += incr_step; break; case VW_DEC: if (Vw - incr_step >= 0) Vw -= incr_step; break; case VW_INC: Vw += incr_step; break; case TW_DEC: if (Tw - incr_step >= 0) Tw -= incr_step; break; case TW_INC: Tw += incr_step; break; case STEP_DEC: if (incr_step > 0) incr_step--; break; case STEP_INC: incr_step++; break; default: break; } lcd_print_info(); VWControl(Vv, Tv, Vw, Tw); KEYGetXY(&x, &y); key_code = KEY_Decode(x, y); } VWExit(); return 0; }