/* PSDCalibrate.c by Omar Anwar, June 2017 PSD (Position Sensitive Device) Callibration Program This is a program to automate the callibratation process of PSDs for LabBots in Robotics Lab, UWA The output of this program will be a text file containing the 128 values of table for the sensor selected at the beggining. This table can be directly copied to hdt.txt and used. ********IMPORTANT******** The output of this program is in mm. Previous HDT had the table values in cm. */ #include #include #include #include #include "eyebot.h" #include "types.h" int main (){ LCDSetPrintf(0, 0,"PSD Calibration Program"); LCDSetPrintf(1, 0, "This program will genrate a text file containing"); LCDSetPrintf(2, 0, "the PSD table used for calibration of PSDs."); LCDSetPrintf(3, 0, "This table can be copied to HDT file for use."); LCDSetPrintf(6, 0, "Choose the PSD you want to Calibrate"); LCDMenu("Left (1)", "Front (2)", "Right (3)", "EXIT"); int k = -1, PSD = 0, i = 0, j=0, temp =0, x=0, y=0; int RAWvalue = 0; int PrevRAWvalue = 0; int RAWvalueVect[7]; int PSDtable[128]; int dist = 300; int previous = 0; FILE *f; k = KEYGet(); //keyread for KEY4 if (k != KEY4) { LCDClear(); if (k == KEY1) { LCDSetPrintf(1, 0, "Calibrating Left Sensor i.e. PSD-1 ..."); LCDSetPrintf(3, 0, "The robot must be at specified dist between left sensor and maze wall"); LCDSetPrintf(5, 0, "Press START to begin the Calibration"); PSD = 1; f = fopen("PSD_Left.txt", "w"); if (f == NULL) { LCDSetPrintf(5, 0, "Error opening file!"); return 0; } fprintf(f, "%s", "TABLE PSD_TableLeft\n"); } if (k == KEY2) { LCDSetPrintf(1, 0, "Calibrating Front Sensor i.e. PSD-2 ..."); LCDSetPrintf(3, 0, "The robot must be at specified distance between front sensor and the maze wall"); LCDSetPrintf(5, 0, "Press START to begin the Calibration"); PSD = 2; f = fopen("PSD_Front.txt", "w"); if (f == NULL) { LCDSetPrintf(5, 0, "Error opening file!"); return 0; } fprintf(f, "%s", "TABLE PSD_TableFront\n"); } if (k == KEY3) { LCDSetPrintf(1, 0, "Calibrating Right Sensor i.e. PSD-3 ..."); LCDSetPrintf(3, 0, "The robot must be at specified distance between right sensor and the maze wall"); LCDSetPrintf(5, 0, "Press START to begin the Calibration"); PSD = 3; f = fopen("PSD_Right.txt", "w"); if (f == NULL) { LCDSetPrintf(5, 0, "Error opening file!"); return 0; } fprintf(f, "%s", "TABLE PSD_TableRight\n"); } } else return 0; LCDMenu("Start", "", "", "EXIT"); k = KEYGet(); if (k == KEY4) // EXIT { return 0; } else // Start Calibration { for (i = 0; i < 128; i++) PSDtable[i] = -1; dist = 300; /*RAWvalue = 0; RAWvalueVect[0] = PSDGetRaw(PSD); // acquire 7 samples RAWvalueVect[1] = PSDGetRaw(PSD); RAWvalueVect[2] = PSDGetRaw(PSD); RAWvalueVect[3] = PSDGetRaw(PSD); RAWvalueVect[4] = PSDGetRaw(PSD); RAWvalueVect[5] = PSDGetRaw(PSD); RAWvalueVect[6] = PSDGetRaw(PSD); for (m = 0; m < 7; m++) // sort them { for (n = m + 1; n < 7; n++) { if (RAWvalueVect[m] > RAWvalueVect[n]) { temp = RAWvalueVect[m]; RAWvalueVect[m] = RAWvalueVect[n]; RAWvalueVect[n] = temp; } } } RAWvalue = (RAWvalueVect[2]+ RAWvalueVect[3]+ RAWvalueVect[4])/3; // AVG 3 middle values RAWvalue = RAWvalue / 32; // pick most significant 7 bits (Range 0 to 127) if (RAWvalue > 0) RAWvalue--; PSDtable[RAWvalue] = dist;*/ previous = 0; } LCDClear(); LCDMenu("Calibrate", "", "", "EXIT"); LCDSetPrintf(3, 0, "Move the robot sensor at a distance of \n\n%d mm", dist); //LCDSetPrintf(0, 0, "%d mm distance written at location %d \n",dist, RAWvalue); k = KEYGet(); while (k!=KEY4) { if (k == KEY1) { RAWvalue = 0; RAWvalueVect[0] = PSDGetRaw(PSD); // acquire 7 samples RAWvalueVect[1] = PSDGetRaw(PSD); RAWvalueVect[2] = PSDGetRaw(PSD); RAWvalueVect[3] = PSDGetRaw(PSD); RAWvalueVect[4] = PSDGetRaw(PSD); RAWvalueVect[5] = PSDGetRaw(PSD); RAWvalueVect[6] = PSDGetRaw(PSD); for (x = 0; x < 7; x++) // sort them { for (y = x + 1; y < 7; y++) { if (RAWvalueVect[x] > RAWvalueVect[y]) { temp = RAWvalueVect[x]; RAWvalueVect[x] = RAWvalueVect[y]; RAWvalueVect[y] = temp; } } } RAWvalue = (RAWvalueVect[2] + RAWvalueVect[3] + RAWvalueVect[4]) / 3; // AVG 3 middle values RAWvalue = RAWvalue / 32; // pick most significant 7 bits (Range 0 to 127) if (RAWvalue > 0) //bring range from 1-128 -> 0-127 RAWvalue--; if (RAWvalue >= PrevRAWvalue) { if (PSDtable[RAWvalue] == -1) // if location empty, put this value PSDtable[RAWvalue] = dist; else // if already filled, put the average of previous and current value PSDtable[RAWvalue] = (PSDtable[RAWvalue] + dist) / 2; PrevRAWvalue = RAWvalue; previous = 0; LCDClear(); LCDMenu("Calibrate", "Previous", "Finish", "EXIT"); LCDSetPrintf(1, 0, "Calibration Done, moving on to next point"); sleep(1); LCDClear(); if (RAWvalue < 127) { LCDMenu("Calibrate", "Previous", "Finish", "EXIT"); LCDSetPrintf(1, 0, "%d mm distance written at location %d \n", PSDtable[RAWvalue], RAWvalue); dist = dist - 10; LCDSetPrintf(3, 0, "Now move the robot sensor at a distance of \n\n%d mm", dist); } else { LCDSetPrintf(1, 0, "%d mm distance written at location %d \n", dist, RAWvalue); LCDSetPrintf(3, 0, "Reached closest value possible, Finishing..."); k = KEY3; sleep(1); } } else { sleep(1); LCDClear(); LCDMenu("Calibrate", "Previous", "Finish", "EXIT"); LCDSetPrintf(1, 0, "Invalid return from PSD, value ignored"); dist = dist - 10; LCDSetPrintf(3, 0, "Now move the robot sensor at a distance of \n\n%d mm", dist); } } if (k == KEY2 && previous == 0) { LCDClear(); LCDMenu("Calibrate", "", "Finish", "EXIT"); dist = dist + 10; LCDSetPrintf(5, 0, "Evaluating previous value again..."); PSDtable[PrevRAWvalue] = -1; LCDSetPrintf(7, 0, "Move the robot sensor back at a distance of \n\n%d mm", dist); previous = 1; } if (k == KEY3 || dist == 0) { LCDClear(); LCDMenu("", "", "", "Done"); PSDtable[0] = 300; if(PSDtable[127] == -1) // if user finished before getiing to last value of table, make last value same as previously acquired distance PSDtable[127] = dist; fprintf(f, "%03d ", PSDtable[0]); for (i = 1; i < 127; i++) { if (PSDtable[i] == -1) { for (j = i+1; j < 128; j++) { if (PSDtable[j] != -1) break; } PSDtable[i] = PSDtable[i - 1]-((PSDtable[i - 1] - PSDtable[j]) / (j - i + 1)); } fprintf(f, "%03d ", PSDtable[i]); if ((i - 7)%20 == 0) fprintf(f, "\n"); } fprintf(f, "%03d ", PSDtable[127]); fprintf(f, "\n"); fprintf(f, "END TABLE\n"); fclose(f); LCDSetPrintf(1, 0, "Finished the Calibration Process"); LCDSetPrintf(2, 0, "Check the text file for PSD Table"); KEYWait(KEY4); return 0; } k = KEYGet(); } return 0; }