From 28804e241aab357403e3e963bcf6b4a4a7de82fb Mon Sep 17 00:00:00 2001 From: knolax <1339802534.kk@gmail.com> Date: Thu, 12 Nov 2015 10:25:18 -0500 Subject: added the files --- main.c | 436 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 436 insertions(+) create mode 100644 main.c (limited to 'main.c') diff --git a/main.c b/main.c new file mode 100644 index 0000000..dde57e5 --- /dev/null +++ b/main.c @@ -0,0 +1,436 @@ +#if 0 +Finch Robot Control (finch.exe) + +Notes: +- finch.exe was compiled for a 32-bit windows O/S +- make sure that the hidapi.dll is in the current working directory +- plug in the finch-usb cable before starting the program + +#endif + + +#include +#include + +#include "Finch.h" + +#ifdef _LINUX_ +#include +#else +#include +#include +#endif + +// use this to output the statistics to a disk file +#define DISKFILE + +// represents 5.0 seconds of history data +#define ENTRIES 50 + +struct hist { + int left; + int right; + float x; + float y; + float z; +}; + +// place to store last 5.0 seconds of sensor information +struct hist snapshot[ENTRIES] = {0}; +int snapofst = 0; +int history = 0; + +// local prototypes +void Help(void); +void Command(char option); +char *CheckForInput(void); +char *GetInput(void); +void Pause(int msec); +void Report(void); +char *AccelText(float x, float y, float z); +char *PosText(float x, float y, float z); + + +/* + * main entrypoint + */ +int main(int argc, char* argv[]) +{ + char *option; + + // attempt to connect to the finch hardware + // ...finch must be already plugged into a usb port + if (Fin_Init() < 0) + exit(1); + + Help(); + + while (1) + { + // throw away any outstanding keyboard characters + CheckForInput(); + + // wait for a new command + // ...accepts a single keypress + printf("\nSelect Command (and press ENTER): "); + option = GetInput(); + + if (*option == 'Q' || *option == 'q') + break; + + // perform the command + Command(*option); + } + + // disconnect before exiting the program + Fin_Exit(); + return(0); +} + + +/* + * display help information + */ +void Help(void) +{ + // all commands are single letters + + printf("\nMovement:\n"); + printf(" F - Forward (F:fast, f:slow)\n"); + printf(" B - Backward (B:fast, b:slow)\n"); + printf(" L - Left (L:tight turn, l:wide turn)\n"); + printf(" R - Right (R:tight turn, r:wide turn)\n"); + + printf("\nControls:\n"); + printf(" Y - Led (Y:on/off, y:solid)\n"); + printf(" Z - Buzzer (Z:on/off, z:continuous)\n"); + + printf("\nSensors (upper case is continuous read):\n"); + printf(" A - Accelerometer\n"); + printf(" O - Obstacle\n"); + printf(" S - Light Sensor\n"); + printf(" T - Temperature\n"); +#ifdef DISKFILE + printf(" H - History Report File (H:collect, h:display)\n"); +#else + printf(" H - History Report (H:collect, h:display)\n"); +#endif + + printf("\n"); + printf(" ?- Help\n"); + printf(" Q- Quit\n"); +} + + +/* + * perform the finch command + */ +void Command(char option) +{ + float fd1,fd2,fd3; + int d1,d2,d3; + char *textp; + + switch (option) + { + case '?': + // display the help message + Help(); + break; + + // motor on/off (F-fwd, B-back, L-left, R-right) + case 'F': + Fin_Motor(8,200,200); + break; + case 'f': + Fin_Motor(21,90,90); + break; + + case 'B': + Fin_Motor(8,-200,-200); + break; + case 'b': + Fin_Motor(21,-90,-90); + break; + + case 'L': + Fin_Motor(5,-140,140); + break; + case 'l': + Fin_Motor(11,80,200); + break; + + case 'R': + Fin_Motor(5,140,-140); + break; + case 'r': + Fin_Motor(11,200,80); + break; + + // acceleration sensors + case 'A': + do + { + Fin_Accel(&fd1,&fd2,&fd3,&d1,&d2); + printf("%s %s Tap:%d Shake:%d \r", PosText(fd1,fd2,fd3), AccelText(fd1, fd2, fd3), d1,d2); + Sleep(300); + } while (!kbhit()); + printf("\n"); + break; + case 'a': + Fin_Accel(&fd1,&fd2,&fd3,&d1,&d2); + printf("%s %s Tap:%d Shake:%d\n", PosText(fd1,fd2,fd3), AccelText(fd1, fd2, fd3), d1,d2); + break; + + // obstacle sensors + case 'O': + do + { + Fin_Obstacle(&d1,&d2); + printf("Obstacle -- Left:%d Right:%d \r", d1,d2); + Sleep(300); + } while (!kbhit()); + printf("\n"); + break; + case 'o': + Fin_Obstacle(&d1,&d2); + printf("Obstacle -- Left:%d Right:%d\n", d1,d2); + break; + + // light sensors + case 'S': + do + { + Fin_Lights(&d1,&d2); + printf("Lights -- Left:%3d Right:%3d \r", d1,d2); + Sleep(300); + } while (!kbhit()); + printf("\n"); + break; + case 's': + Fin_Lights(&d1,&d2); + printf("Lights -- Left:%3d Right:%3d\n", d1,d2); + break; + + // temperature sensors + case 'T': + do + { + Fin_Temp(&fd1); + printf("%3.4f Celsius \r", fd1); + Sleep(1000); + } while (!kbhit()); + printf("\n"); + break; + case 't': + Fin_Temp(&fd1); + printf("%3.4f Celsius\n", fd1); + break; + + // led colors (blink or continuous) + case 'Y': + case 'y': + printf("Enter the three color values: red,green,blue (0-255): "); + textp = GetInput(); + sscanf(textp,"%d,%d,%d",&d1,&d2,&d3); + do + { + Fin_LED(d1,d2,d3); + if (option == 'y') + break; + Sleep(250); + Fin_LED(0,0,0); + Sleep(250); + } while (!kbhit()); + break; + case 'C': + d2 = maxfinchnum(); + printf("Enter the number of the finch you want to control 0-%d\n",d2); + textp = GetInput(); + sscanf(textp,"%d",&d1); + if (switchfinch(d1) == 0) { + printf("error not in range\n"); + } + break; + // buzzer (blink or continuous) + case 'Z': + case 'z': + printf("Enter the buzzer frequency: "); + textp = GetInput(); + sscanf(textp,"%d",&d1); + do + { + Fin_Buzzer(0xffff,d1); + if (option == 'z') + break; + Sleep(250); + Fin_Buzzer(0,0); + Sleep(250); + } while (!kbhit()); + break; + + // motor and acceleration history + case 'H': + history = 1; + printf("Reporting on\n"); + break; + case 'h': + history = 0; + Report(); + break; + } +} + + +/* + * check for a keypress + */ +char *GetInput(void) +{ + // check for incoming command + // collect history if enabled + while (!kbhit()) + Pause(100); + + return(CheckForInput()); +} + + +/* + * pause and do the background history collection + */ +void Pause(int msec) +{ + float fd1,fd2,fd3; + int d1,d2; + + while (msec > 0) + { + Sleep(100); + msec -= 100; + + if (history == 0) + continue; + + // collect acceleration data + Fin_Accel(&fd1,&fd2,&fd3,&d1,&d2); + if (++snapofst >= ENTRIES) + snapofst = 0; + + // save history of what was detected + Fin_Speed(&snapshot[snapofst].left,&snapshot[snapofst].right); + snapshot[snapofst].x = fd1; + snapshot[snapofst].y = fd2; + snapshot[snapofst].z = fd3; + } +} + + +/* + * display the history report + */ +void Report(void) +{ + int indx,max; +#ifdef DISKFILE + FILE *disk; + + disk = fopen("Finch.Txt", "w+"); + if (disk == NULL) + return; +#endif + + // display a report of what happened in the last ENTRIES x 1/10 seconds + indx = snapofst; + for (max=1; max<=ENTRIES; max++) + { + if (++indx >= ENTRIES) + indx = 0; + +#ifdef DISKFILE + fprintf(disk,"Time:%3d.%d Left:%4d Right:%4d %s\n", + max/10, max%10, snapshot[indx].left, snapshot[indx].right, + AccelText(snapshot[indx].x, snapshot[indx].y, snapshot[indx].z)); +#else + printf("Time:%3d.%d Left:%4d Right:%4d %s\n", + max/10, max%10, snapshot[indx].left, snapshot[indx].right, + AccelText(snapshot[indx].x, snapshot[indx].y, snapshot[indx].z)); +#endif + } + +#ifdef DISKFILE + fclose(disk); + system("type Finch.txt"); +#endif + +} + + +struct posfmt { + float min_x; + float max_x; + float min_y; + float max_y; + float min_z; + float max_z; + char *desc; +}; + +/* determine if the x,y,z acceleration values are within range */ +int PosTest(struct posfmt *pf, float x, float y, float z) +{ + if (x <= pf->min_x) + return(0); + if (x >= pf->max_x) + return(0); + + if (y <= pf->min_y) + return(0); + if (y >= pf->max_y) + return(0); + + if (z <= pf->min_z) + return(0); + if (z >= pf->max_z) + return(0); + + return(1); +} + +char *PosText(float x, float y, float z) +{ + static struct posfmt PosTbl[9] = { + { -1.5, -0.8, -0.3, 0.3, -0.3, 0.3, "Beak-Up " }, + { 0.8, 1.5, -0.3, 0.3, -0.3, 0.3, "Beak-Dn " }, + { 0.2, 0.5, -0.3, 0.3, 0.9, 1.2, "Dn-Hill " }, + { -0.5, -0.2, -0.3, 0.3, 0.9, 1.2, "Up-Hill " }, + { -0.5, 0.5, -0.5, 0.5, 0.65, 1.2, "Level " }, + { -0.5, 0.5, -0.5, 0.5, -1.5, -0.65,"Dn/Up " }, + { -0.5, 0.5, 0.7, 1.5, -0.5, 0.5, "LftWing-Dn" }, + { -0.5, 0.5, -1.5,-0.7, -0.5, 0.5, "RgtWing-Dn" }, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, " " } + }; + char *textp; + int ofst; + + /* convert an x,y,z acceleration to a description */ + textp = PosTbl[8].desc; + for (ofst=0; ofst<8; ofst++) + { + if (PosTest(&PosTbl[ofst], x, y, z)) + { + textp = PosTbl[ofst].desc; + break; + } + } + + return(textp); +} + +char *AccelText(float x, float y, float z) +{ + static char text[80]; + + /* display the x,y,z acceleration values */ + sprintf(text,"X:%2.5f Y:%2.5f Z:%2.5f",x,y,z); + return(text); +} -- cgit v1.1