main.cpp

00001 #include "mbed.h"
00002 #include "USBHID.h"
00003 #include "ByteOperations.h"
00004 #include "USBHIDProtocol.h"
00005 
00006 #define COMMAND 0
00007 #define DATA 1
00008 
00009 #define AVERAGE       0
00010 #define MEDIAN        1
00011 #define BINAVERAGE    2
00012 #define RATEOFCHANGE  3
00013 
00014 #define VERSION 0x01
00015 
00016 Serial pc(USBTX, USBRX);
00017 DigitalOut led1(LED1);
00018 DigitalOut led2(LED2);
00019 DigitalOut led3(LED3);
00020 DigitalOut led4(LED4);
00021 AnalogIn opacity(p20);
00022 AnalogOut fOpacity(p18);
00023 BusInOut databus(p8, p9, p10, p11, p12, p13, p14, p15);
00024 DigitalOut registerSelect(p5);
00025 DigitalOut readWriteClock(p7);
00026 DigitalOut readWrite(p6);
00027 //DigitalIn unlocked(p8);
00028 Ticker updateLCD;
00029 Ticker processOpacity;
00030 LocalFileSystem local("local");
00031 
00032 USBHID *hid;
00033 HID_REPORT send_report __attribute__((aligned (4))); // Aligned for fast access
00034 HID_REPORT recv_report __attribute__((aligned (4))); // Aligned for fast access
00035 
00036 float instantOpacity;
00037 float filteredOpacity = 0.0;
00038 float Opacity = 0.0;
00039 char *helloStr = "Hello";
00040 int filterAlgorithm = AVERAGE;
00041 float anIn1Sum;
00042 float anIn1SumSqr;
00043 float stdDevCount;
00044 float standardDeviation;
00045 float calibFactor = 1.0;
00046 int showCalibFactor = 0;
00047 float anInVals[100];
00048 int anInIdx = 0;
00049 float binVal[10];
00050 int binCnt[10];
00051 int maxCnt = 0;
00052 int maxIdx = 0;
00053 
00054 
00055 void readConfigFile()
00056 {
00057     
00058     FILE *fp = fopen("/local/config.dat", "r");
00059 
00060     if (fp != NULL)
00061     {
00062         fscanf(fp, "%f", &calibFactor); 
00063         fscanf(fp, "%d", &filterAlgorithm); 
00064         fclose(fp);
00065     }
00066 }
00067 
00068 
00069 
00070 void writeConfigFile()
00071 {
00072     FILE *fp = fopen("/local/config.dat", "w");
00073 
00074     if (fp != NULL)
00075     {
00076         fprintf(fp, "%5.3f\n", calibFactor);
00077         fprintf(fp, "%1d\n", filterAlgorithm);
00078         fclose(fp);
00079     }
00080 }
00081 
00082 
00083 
00084 void empty_report(HID_REPORT *data){
00085     register uint32_t *p = (uint32_t *)data->data;
00086     for( register int i=0; i<((sizeof(HID_REPORT)-1)/4); i++ ){
00087         *p = 0xFFFFFFFF;
00088         p++;
00089     }
00090 }
00091 
00092 void checkForUSBRequest()
00093 {
00094     char *cptrR;
00095     char *cptrT;
00096     float fTmp;
00097     //bool updatePIDValues = false;
00098 
00099     //try to read a msg
00100     if(hid->readNB(&recv_report)){
00101         
00102         // set character pointer to start of the parameter for set commands
00103         cptrR = (char *)&recv_report.data[1];
00104         cptrT = (char *)&send_report.data[1];
00105 
00106         led2 = 1;
00107         
00108         // Data packet received, start parsing
00109         int irx=0;
00110         int itx=0;
00111 
00112         send_report.data[itx++] = recv_report.data[0];
00113 
00114         switch ( recv_report.data[irx++] ){
00115             case    CMD_SYS_CHECK       :
00116                 send_report.data[itx++] =  VERSION;
00117                 break;
00118             
00119             case    CMD_SYS_RESET       : 
00120                   // Soft reset
00121                     empty_report(&recv_report);
00122                 break;
00123 
00124             case CMD_GET_RAW_OPACITY:
00125                 // return the raw opacity value
00126                 sprintf(cptrT, "%6.5f", instantOpacity * 100.0);
00127                 pc.printf("instant opacity = %f\n", instantOpacity * 100.0);
00128                 break;
00129             case CMD_GET_FILTERED_OPACITY:
00130                 // return filtered opacity
00131                 sprintf(cptrT, "%6.5f", Opacity);
00132                 pc.printf("filtered opacity = %f\n", Opacity);
00133                 break;
00134             case CMD_GET_CALIB_FACTOR:
00135                 // return calibration factor
00136                 sprintf(cptrT, "%6.5f", calibFactor);
00137                 pc.printf("calibration factor = %f\n", calibFactor);
00138                 break;
00139             case CMD_SET_CALIB_FACTOR:
00140                 // set calibration factor to value in packet
00141                 sscanf(cptrR, "%f", &calibFactor);
00142                 writeConfigFile();
00143                 pc.printf("Set calibFactor to %f\n", calibFactor);
00144                 break;
00145             case CMD_GET_FILTER_MODE:
00146                 // return filter algorithm value
00147                 sprintf(cptrT, "%6.3f", (float)filterAlgorithm);
00148                 pc.printf("filterAlgorithm = %d\n", filterAlgorithm);
00149                 break;
00150             case CMD_SET_FILTER_MODE:
00151                 // set filter alogirthm
00152                 sscanf(cptrR, "%f", &fTmp);
00153                 filterAlgorithm = (int)fTmp;
00154                 writeConfigFile();
00155                 pc.printf("Set filter algorithm to %d\n", filterAlgorithm);
00156                 break;
00157             case 0xEE   : {
00158                 hid->sendNB(&send_report);  
00159                 //WatchDog_us bello(100);
00160                 }
00161             break;
00162 
00163             default:
00164                 send_report.data[0] =   0xFF;   //Failure
00165             break;
00166         } // Switch 
00167 
00168         // Return command + optional new args
00169         hid->send(&send_report);
00170 
00171         // 0xFF unused bytes
00172         empty_report(&recv_report);         
00173         empty_report(&send_report);
00174         
00175         led2 = 0;
00176     }       // if packet
00177 }
00178 
00179 
00180 
00181 void writeToLCD(bool rs, char data){
00182 
00183     // set register select pin 
00184     registerSelect = rs;
00185     
00186     // set read/write pin to write
00187     readWrite = 0;
00188     
00189     // set bus as output
00190     databus.output();
00191     
00192     // put data onto bus
00193     databus = data;
00194     
00195     // pulse read/write clock
00196     readWriteClock = 1;
00197     
00198     wait_us(1);
00199     
00200     readWriteClock = 0;
00201 
00202     wait_us(1);
00203     
00204     // clear data bus
00205     databus = 0;
00206     
00207     //pc.printf("%02x\n", data);
00208     
00209 }
00210 
00211 
00212 char readFromLCD(bool rs){
00213 
00214     char data;
00215     
00216     // set register select pin 
00217     registerSelect = rs;
00218     
00219     // set read/write pin to read
00220     readWrite = 1;
00221     
00222     // set bus as output
00223     databus.input();
00224     
00225     // put data onto bus
00226     data = databus;
00227     
00228     // pulse read/write clock
00229     readWriteClock = 1;
00230     
00231     wait_us(10);
00232     
00233     readWriteClock = 0;
00234     
00235     return data;
00236 }
00237 
00238 
00239 void resetLCD(){
00240 }
00241 
00242 
00243 void initLCD(){
00244     
00245     // wait 15 ms to allow LCD to initialise
00246     wait_ms(15);
00247     
00248     // set interface for 8 bit mode
00249     writeToLCD(COMMAND, 0x30);
00250 
00251     // give it time    
00252     wait_ms(5);
00253 
00254     // set interface for 8 bit mode again
00255     writeToLCD(COMMAND, 0x30);
00256 
00257     // give it time    
00258     wait_us(100);
00259 
00260     // set interface for 8 bit mode again, last one before we can configure the display
00261     writeToLCD(COMMAND, 0x30);
00262 
00263     // give it time    
00264     wait_us(500);
00265     
00266     // set interface for 8 bit mode, 2 display lines and 5 x 8 character font
00267     writeToLCD(COMMAND, 0x38);
00268 
00269     // give it time    
00270     wait_us(100);
00271     
00272     // display off
00273     writeToLCD(COMMAND, 0x08);
00274 
00275     // give it time    
00276     wait_us(100);
00277     
00278     // clear the screen
00279     writeToLCD(COMMAND, 0x01);
00280 
00281     // give it time to finish    
00282     wait_ms(2);
00283 
00284     // set entry mode to increment cursor position cursor on write
00285     writeToLCD(COMMAND, 0x03);
00286     
00287     // give it time to finish    
00288     wait_us(100);
00289 
00290     // position cursor at home
00291     writeToLCD(COMMAND, 0x02);
00292     
00293     // give it time to finish    
00294     wait_ms(2);
00295 
00296     // display on
00297     writeToLCD(COMMAND, 0x0F);
00298 }
00299 
00300 
00301 
00302 
00303 void positionCursor(uint8_t x, uint8_t y){
00304 
00305     if (x > 7) x = 0;
00306     
00307     if (y == 1)
00308         writeToLCD(COMMAND, 0x80 + 0x40 + x);
00309     else
00310         writeToLCD(COMMAND, 0x80 + 0x00 + x);
00311         
00312     wait_us(50);
00313 }
00314 
00315 
00316 void displayString(int x, int y, char *str){
00317     
00318     // position cursor
00319     positionCursor(x, y);    
00320     
00321     // write string to screen
00322     for (int i=0; i<strlen(str); i++){
00323         writeToLCD(DATA, str[i]);
00324         
00325         wait_us(50);
00326     }
00327 }
00328 
00329 
00330 void standardDeviationCalc(float opacity)
00331 {
00332     // add to standard deviation accumulators
00333     anIn1Sum += opacity;
00334     anIn1SumSqr += (opacity * opacity);
00335     
00336     // increment standard deviation counter
00337     stdDevCount++;
00338     
00339     // if enough readings for the standard deviation calculation
00340     if (stdDevCount >= 100)
00341     {
00342         // calculate the standard deviation
00343         // std dev = sqrt( (n * sum(x2) - sum(x)2)) / (n * (n - 1)))
00344         standardDeviation = ((stdDevCount * anIn1SumSqr) - (anIn1Sum * anIn1Sum)) / (stdDevCount * (stdDevCount - 1));
00345         if (standardDeviation > 0.0)
00346             standardDeviation = sqrt(standardDeviation);
00347         else
00348             standardDeviation = sqrt(-standardDeviation);
00349         
00350         // clear standard deviation accumulators for next set of readings
00351         anIn1Sum = 0.0;
00352         anIn1SumSqr = 0.0;
00353         stdDevCount = 0;
00354     }
00355  }
00356  
00357  
00358  void updateDisplay(){
00359     char str[20];
00360     
00361     sprintf( str, "o %5.1f", Opacity);
00362 
00363     displayString(0, 0, str);
00364     
00365     //if (showCalibFactor == 0){
00366         //sprintf( str, "s %5.2f", standardDeviation);
00367 
00368         //displayString(0, 1, str);
00369     //}
00370     //else{
00371         //sprintf( str, "m %5.2f", calibFactor);
00372 
00373         //displayString(0, 1, str);
00374 
00375         //showCalibFactor--;
00376     //}
00377 }
00378  
00379  
00380  void updateOpacity()
00381 {
00382 
00383    // read next analog input value into circular buffer, adjust reading for max value is 3.1 on 3.3V input
00384     anInVals[anInIdx] = opacity.read() * 1.0674;
00385     
00386     // increment anInIdx and check for wrap 
00387     anInIdx++;
00388     if (anInIdx >= 100)
00389         anInIdx = 0;
00390     
00391     // filter analog inputs with required algorithm
00392     switch (filterAlgorithm)
00393     {
00394         case AVERAGE:
00395             float accumulator = 0.0;
00396             for (int i=0; i<100; i++)
00397             {
00398                 accumulator += anInVals[i];
00399             }
00400             instantOpacity = accumulator / 100;
00401             break;
00402         case MEDIAN:
00403             float tempF;
00404             for (int j=1; j<100; j++)
00405             {
00406                 for (int i=1; i<100; i++)
00407                 {
00408                     if (anInVals[i] < anInVals[i-1])
00409                     {
00410                         // swap places
00411                         tempF = anInVals[i-1] ;
00412                         anInVals[i-1] = anInVals[i];
00413                         anInVals[i] = tempF;
00414                     }
00415                 }
00416             }
00417             instantOpacity = anInVals[49];
00418             break;
00419         case BINAVERAGE:
00420             // initialise bins to zero
00421             for (int i=0; i<10; i++)
00422             {
00423                 binVal[i] = 0.0;
00424                 binCnt[i] = 0;
00425             }
00426             
00427             // sort analog input values into one of ten bins
00428             for (int i=0; i<100; i++)
00429             {
00430                 int binIdx = anInVals[i] * 10.0;
00431                 if (binIdx > 9) 
00432                     binIdx = 9;
00433                 binVal[binIdx] += anInVals[i];
00434                 binCnt[binIdx]++;
00435             }
00436 
00437             maxCnt = 0;
00438             maxIdx = 0;
00439             // find the bin with most values added
00440             for (int i=0; i<10; i++)
00441             {
00442                 if (binCnt[i] > maxCnt)
00443                 {
00444                     maxCnt = binCnt[i];
00445                     maxIdx = i;
00446                 }
00447             }
00448             
00449             instantOpacity = binVal[maxIdx] / binCnt[maxIdx];
00450             break;
00451         case RATEOFCHANGE:
00452             break;
00453         default:
00454             break;
00455     }
00456 
00457     // do standard deviation on the smoothed opacity value
00458     standardDeviationCalc(instantOpacity);
00459 
00460     // apply a filter to the instant reading to get the filtered reading
00461     filteredOpacity = (instantOpacity * 0.05) + (filteredOpacity * 0.95);
00462 
00463     // calculate opacity reading as 0..100%
00464     Opacity = filteredOpacity * calibFactor * 100.0;
00465     
00466     // write opacity value to analog output as 0..1.0 value
00467     fOpacity.write(Opacity / 100.0);
00468 }
00469     
00470  
00471 int main() {
00472     
00473     printf("initLCD()\n");
00474     
00475     initLCD();
00476     
00477     filterAlgorithm = BINAVERAGE;
00478     
00479     //calibUp.mode(PullUp);
00480     //calibDown.mode(PullUp);
00481     //unlocked.mode(PullUp);
00482     //calibUp.attach_deasserted(&incCalibFactor);
00483     //calibUp.attach_deasserted_held(&incCalibFactor);
00484     //calibDown.attach_deasserted(&decCalibFactor);
00485     //calibDown.attach_deasserted_held(&decCalibFactor);
00486     
00487     //calibUp.setSampleFrequency();
00488     //calibDown.setSampleFrequency();
00489 
00490     printf("readConfigFile()\n");
00491 
00492     readConfigFile();
00493 
00494     printf("start updateLCD ticker\n");
00495 
00496     updateLCD.attach(&updateDisplay, 0.5);
00497 
00498     printf("read opacity 10 times\n");
00499 
00500     // initialise analog input values
00501     for (int i=0; i<100; i++)
00502         anInVals[i] = opacity.read();
00503         
00504     printf("start processOpacity ticker\n");
00505 
00506     // start ticker to read and filter the opacity input
00507     processOpacity.attach(&updateOpacity, 0.1);
00508         
00509     printf("initialise USB\n");
00510 
00511     // USB Initialize   
00512     static USBHID hid_object(64, 64);
00513     hid = &hid_object;
00514     send_report.length = 64;
00515 
00516     while(1){
00517 
00518         // check for any commands from host computer
00519         checkForUSBRequest();
00520         
00521         wait_ms(10);
00522     }
00523 
00524 }