Marc Bax
/
Sorbonne2
Sorbonne 2 firmware
main.cpp
- Committer:
- marcbax
- Date:
- 2011-07-25
- Revision:
- 0:0993f4cf5312
File content as of revision 0:0993f4cf5312:
//Firmware to drive Sorbonne-2 bee training module char* fwversion = "0.70"; //For published versions use format "n.nn" //For development versions add "D" to end of string //This version published on 25/07/2011 #include "mbed.h" #include "Servo.h" DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4); DigitalOut lidclose(p5), lidlift(p6); DigitalOut clamp(p7), unclamp(p8); DigitalOut lockr(p12), unlockl(p13), lockl(p14), unlockr(p26); AnalogIn viclamp(p15); AnalogIn vswdetect(p16); AnalogIn adc4(p17); AnalogOut irLED(p18); AnalogIn vtemp(p19); AnalogIn ptor(p20); Servo arm(p21), elbow(p22); DigitalOut fan(p23); I2C beeholder(p9, p10); DigitalOut bhsel(p11); Serial acucomms(p28, p27); DigitalIn tmselect(p29); DigitalOut airswitcher(p25), spare1(p24); LocalFileSystem local("local"); //allow access to the mbed "thumbdrive" as a file system //FILE *calfile = fopen("/local/calfile.ini","w"); //file used to store bee holder calibration values FILE *logfile = fopen("/local/sorb2log.csv","w"); //creates new file sorb2log.csv and set for writing. Overwrites existing file! //Definition of global constants //Training module PCB #2 robot arm positions //Increase in arm position moves arm away from bee holder //Increase in elbow position lowers tip float armbasepos = 0.81; float elbowbasepos = 0.6; float armdippos = 0.81; float elbowdippos = 0.84; float armticklepos = 0.15; float elbowticklepos = 0.63; float armfeedpos = 0.08; float elbowfeedpos = 0.77; //robot arm hold and move times //all times are in seconds float diptime = 1; //time that brush stays down dipped in sugar water float tickletimeout = 3; //maximum time to tickle if no PER is detected float feedtime = 3; //feeding time //PER detection parameters float calsetval = 1.4; //voltage to which phototransistor output will be calibrated float PERsetval = 2.1; //phototransistor output needs to be above this voltage for PER float PERtime = 0.2; //phototransistor output needs to stay high this long in seconds //Protocols int addr = 0xA2; //I2C address is actually 0x51, but mbed expects left-shifted address int I2Cfreq = 10000; //I2C bus frequency int baudrate = 19200; //baudrate of serial connection to ACU //Definition of global variables struct holderrecord { //used to hold data on beeholder int serialno; //beeholder serial number 0-9999 float calvalue; //IR-LED calibration value char cycleno; //number of cycles gone through in this session char reslastcycle; //result of last cycle, 1 for PER char ticklenextcycle; //whether tickling was done for last cycle time_t tstamp; //timestamp when last cycle was run }; holderrecord currentholder; //struct record for the current beeholder holderrecord sessionrecord[101]; //sessionrecord contains holderrecords for up to 100 beeholders int numbeeholders; //number of beeholders used in current session int serialnum; char comchar; //serial command character for module control float fanspeed; bool lockstatus, clampstatus, lidstatus; //are true when locked, clamped or closed char swstatus; //4-bit number to indicate status of 4 detect switches float swvalue; //ADC value for detect switch voltage //Function declarations //Initialises sessionrecords void sessionrecordinit() { for (int i=0; i<101; i++) { //set base values for all possible beeholders sessionrecord[i].serialno = 0; //set serialno to 0 so we can detect a unresponsive holder sessionrecord[i].calvalue = 0; sessionrecord[i].cycleno = 0; sessionrecord[i].reslastcycle = 0; sessionrecord[i].ticklenextcycle = 1; //default is to tickle on each cycle } } //Reads serial numbers and calibration values from calfile.ini void getcaldata() { } //Initialise at power-up or reset void init_tm() { led1=led2=led3=0; led4=1; lidclose=lidlift=0; clamp=unclamp=0; lockr=unlockr=lockl=unlockl=0; irLED=0; bhsel=0; airswitcher=0; spare1=0; fan=0; acucomms.baud(baudrate); beeholder.frequency(I2Cfreq); comchar=0; lockstatus=0; numbeeholders=0; sessionrecordinit(); acucomms.printf("\n\r\rFirmware version: %s", fwversion); } //Converts the last 4 digits in the serial number string into a integer 0-9999 int serialstring2int(char bser[8]) { int tempserial = 0; tempserial = tempserial + (bser[4]-0x30)*1000; //5th digit is thousands tempserial = tempserial + (bser[5]-0x30)*100; //6th digit is hundreds tempserial = tempserial + (bser[6]-0x30)*10; //7th digit is tens tempserial = tempserial + (bser[7]-0x30); //8th digit is units return tempserial; } //beeholder resets on rising edge of select line void resetbeeholder() { //need this as mounting beeholder causes undefined start of beeholder firmware bhsel = 0; wait(0.1); bhsel = 1; wait(0.3); } //gets holderrecord index for the holder with a certain serial number int getholderindex(int serialno) { for (int i=1; i<=100; i++) { //reverse lookup of index for a certain serial number if (sessionrecord[i].serialno == serialno) return i; } return 0; //if the record is not found, returns i==0 } //Reads beeholder serial number int getserialno() { char bser[8]; //define 8-byte serial number string to read resetbeeholder(); //does not work without this!! for (int i=0;i<8;i++) bser[i]=0x30; bhsel = 0; //pull select line low wait(0.2); beeholder.stop(); //I2C stop condition wait(0.2); //delay for beeholder to respond beeholder.write(addr,0x0,1); //initial write before read beeholder.read(addr,bser,8); //read 8 byte serial number bhsel = 1; //pull select line high int serialno = serialstring2int(bser); //translate serial number string to integer return serialno; } //Returns 1 if a PER is detected within timeout seconds int detectPER(float timeout) { Timer ttotal, tper; ttotal.start(); //start timers for time-out and PER-detect ttotal.reset(); tper.start(); tper.reset(); while ((ttotal.read() < timeout) && (tper.read() < PERtime)) { //loop until timeout or PER detected wait_ms(10); if (ptor * 3.3 < PERsetval) tper.reset(); //if phototransistor voltage below treshold keep PER timer in reset //if above treshold let timer run until it reaches PERtime } ttotal.stop(); tper.stop(); return (tper.read() >= PERtime); //if the loop exit condition was a PER, return a TRUE value } //Function performs beeholder/IR-led calibration float calibrate(int runs) { float tempcal, ptorhold; tempcal=0.5; //start calibration at 50% voltage irLED=tempcal; float calstep; //start calstep at 25% voltage calstep = tempcal/2; //acucomms.printf("\n\rInitial calibration:"); for (int i=0; i<10; i++) { //does a "10-bit binary search" for the correct voltage to get a good response irLED = tempcal; wait(0.1); //important to allow AD converter to settle ptorhold=ptor; //acucomms.printf("\n\r%5.3f - %5.3f", tempcal, ptorhold); if (ptorhold < calsetval/3.3) { //check phototransistor voltage against desired value tempcal = tempcal - calstep; //if phototransistor voltage is too low then reduce brightness } else { tempcal = tempcal + calstep; //if phototransistor voltage is too high then increase brightness } calstep = calstep/2; //on each loop of the for-cycle make smaller changes to IR LED voltage } float calib; calib = tempcal; //set preliminary calibration to the value just measured for (int j=1; j<runs; j++) { //run another j-1 runs, this corrects for antennae-movement as tempcal=0.5; //we use the lowest calibration value from j runs irLED=tempcal; //this is similar to what we do in the cassettes calstep = tempcal/2; for (int i=0;i<10;i++) { irLED = tempcal; wait(0.1); ptorhold=ptor; if (ptorhold < calsetval/3.3) { tempcal = tempcal - calstep; } else { tempcal = tempcal + calstep; } calstep = calstep/2; } if (tempcal < calib) calib = tempcal; //use the lowest of j calibration values } irLED = 0; return calib; } //switches the IR LED on at the right brightness level for the beeholder void IRledON(int i) { irLED = sessionrecord[i].calvalue; } //moves arm to a position in a specified time, allowing speed control void armmove (float endpos, float movetime) { float startpos = arm.read(); int numsteps = (movetime * 50); //50 pulses/second, so each step is 1 servo pulse for (int i=0; i<numsteps; i++) { arm = startpos + i * (endpos - startpos)/numsteps; wait(movetime/numsteps); } arm = endpos; } //moves elbow to a position in a specified time, allowing speed control void elbowmove (float endpos, float movetime) { float startpos = elbow.read(); int numsteps = (movetime * 50); //50 pulses/second, so each step is 1 servo pulse for (int i=0; i<numsteps; i++) { elbow = startpos + i * (endpos - startpos)/numsteps; wait(movetime/numsteps); } elbow = endpos; } //Performs a conditioning cycle. if tickle==0, no tickling takes place. Return==1 if a PER has been detected int condcycle(int tickle) { int perseen; perseen = 0; //dip brush armmove(armdippos, 0.5); elbowmove(elbowdippos, 0.3); wait(diptime); elbowmove(elbowbasepos, 0.3); armmove(armbasepos, 0.5); airswitcher=1; //switch air to target odour //tickle if (tickle) { //if tickling, first tickle then wait for PER or timeout elbowmove(elbowticklepos, 0.2); armmove(armticklepos, 1.5); //slower move of arm towards bee perseen = detectPER(tickletimeout); //tickle until timeout or PER detected } //or not tickle else { perseen = detectPER(tickletimeout); //if not tickling, wait for PER or timeout then move to pre-feeding position armmove(armticklepos-0.05, 1); //move to position between LED and holder elbowmove(elbowticklepos, 0.3); } //feeding only if you have tickled or a PER has been detected if (tickle || perseen) { //only feed if a PER has been detector, or "tickle" is true elbowmove(elbowfeedpos, 0.3); armmove(armfeedpos, 0.3); wait(feedtime); armmove(armticklepos -0.05, 0.3); elbowmove(elbowticklepos, 0.3); } //move back to base position airswitcher=0; //switch back to clean air armmove(armbasepos, 0.5); //back to basepos elbowmove(elbowbasepos, 0.3); // return perseen; } char switchstatus(float vswdetect) { //outputs char (4-bit flag) to reflect positions of 4 detect switches if (vswdetect>0.38 && vswdetect<0.4) return 11; if (vswdetect>0.4 && vswdetect<0.45) return 9; if (vswdetect>0.45 && vswdetect<0.49) return 10; if (vswdetect>0.49 && vswdetect<0.54) return 8; if (vswdetect>0.54 && vswdetect<0.58) return 7; if (vswdetect>0.58 && vswdetect<0.62) return 5; if (vswdetect>0.62 && vswdetect<0.66) return 3; if (vswdetect>0.66 && vswdetect<0.74) return 1; if (vswdetect>0.74 && vswdetect<0.8) return 6; if (vswdetect>0.8 && vswdetect<0.85) return 4; if (vswdetect>0.85 && vswdetect<0.95) return 2; if (vswdetect>0.95 && vswdetect<1.01) return 0; return 16; } //Clamp bee holder void clampholder() { unclamp=1; wait(4); unclamp=0; } //Unclamp bee holder void unclampholder() { clamp=1; wait(1); clamp=0; } //Lift lid void liftlid() { bool notup; notup=1; lidclose=1; while (notup) { wait(0.01); notup = (switchstatus(vswdetect.read()) != 7); } lidclose=0; lidstatus=0; } //Close lid void closelid() { bool notdown; notdown=1; lidlift=1; while (notdown) { wait(0.01); notdown = (switchstatus(vswdetect.read()) != 11); } lidlift=0; lidstatus=1; } //Lock lid void locklid() { wait(0.1); } //Unlock lid void unlocklid() { wait(0.1); } //Set date and time void setdatetime() { } void registerbeeholder() { //registers and calibrates the bee holder currently clamped int serialno, i; float calvalue; serialno = getserialno(); i = getholderindex(serialno); if (i == 0) { numbeeholders++; if (numbeeholders == 101) { acucomms.printf("Number of holders exceeds 100"); } else { sessionrecord[numbeeholders].serialno = serialno; calvalue = calibrate(5); if (calvalue < 0.98 && calvalue > 0.25) { sessionrecord[numbeeholders].calvalue = calvalue; acucomms.printf("\n\rCal %4u - %4.2fV", serialno, calvalue*3.3); } else { acucomms.printf("\n\rCal %4u - invalid", serialno); } } } } /* //Registers and calibrates all beeholders used in this session int registerbeeholders() { int i; bool done; char buffert[30]; i = done = 0; //cleardisplay(); //clear screen and home cursor fprintf(logfile, "calibration record:\r"); fprintf(logfile, "i, serialno, LED V, time\r"); while (i<30 && !done) { //register and calibrate a maximum of 30 beeholders //display.printf("calibrating %u\r",i+1); sessionrecord[i].serialno = getserialno(); //read serial number if (sessionrecord[i].serialno != 0) { //check if serial number correctly read - if not it will be 0000 sessionrecord[i].calvalue = calibrate(station, 5); //5 calibration cycles if ((sessionrecord[i].calvalue > 0.25) && (sessionrecord[i].calvalue < 0.97)) { //check that calvalue is in expected range sessionrecord[i].tstamp = time(NULL); //create timestamp NOW strftime(buffert, 20, "%X", localtime(&sessionrecord[i].tstamp)); //formats time part of timestamp cleardisplay(); display.printf("SN %4u - cal %4.2fV\r", sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3); display.printf("OK for next\r"); display.printf("DOWN for training"); fprintf(logfile, "%4u,%6u,%6.2f, %s, calibrated\r", i, sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3, buffert); i++; } else { cleardisplay(); display.printf("SN %4u - cal %4.2fV\r", sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3); display.printf("Cal out of range!\r"); multibeeps(2,0.5); display.printf("OK to recalibrate\r"); fprintf(logfile, "%4u,%6u,%6.2f, %s, out of range\r", i, sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3, buffert); } while (!done && oksw) { //loop until OK or DOWN are pressed wait(0.02); done = !dnsw; //DOWN exits registration cycle and moves to training } } else { //retry when serialno can't be read (is 0000) cleardisplay(); multibeeps(3,0.3); //beep-beep-beep when beeholder not correctly read display.printf("invalid serial no\r"); display.printf("reseat beeholder\r"); display.printf("press OK"); while (oksw) wait(0.02); //loop until OK is pressed to start calibration loop again } cleardisplay(); } return i; //upon done condition, i== number of beeholders calibrated } */ float calcairtemp(float vntc) { return 0.1; } //all the elements making up a single training cycle with one beeholder void trainingcycle() { wait(0.2); time_t tstamp = time(NULL); //create timestamp NOW char buffert[30]; strftime(buffert, 20, "%X", localtime(&tstamp)); //format timestamp to time string int serialno; serialno = getserialno(); int i = getholderindex(serialno); //get index i for serial number if (i != 0) { IRledON(i); //switch IR LED on at correct brightness sessionrecord[i].cycleno++; //increment cycle number for this beeholder acucomms.printf("\n\rSN: %4u, cycle %u - ", serialno, sessionrecord[i].cycleno); sessionrecord[i].reslastcycle = condcycle(sessionrecord[i].ticklenextcycle); //do a conditioning cycle fprintf(logfile, "%s,",buffert); fprintf(logfile, " %4u,",serialno); fprintf(logfile, " %2u,", sessionrecord[i].cycleno); if (sessionrecord[i].reslastcycle) { //log PER or TimeOut fprintf(logfile, " PER,"); } else { fprintf(logfile, " TO,"); } fprintf(logfile, " training\r"); if (sessionrecord[i].reslastcycle) { acucomms.printf("PER detected"); } else { acucomms.printf("PER time-out"); } } else { acucomms.printf("\n\rBee holder %4u not registered", serialno); } } int main() { init_tm(); while (comchar != 88) { comchar = acucomms.getc(); switch (comchar) { case 65: //"A" //Continue after pause break; case 66: //"B" //Register bee holder registerbeeholder(); break; case 67: //"C" //Calibrate bee holder acucomms.printf("\r\nCalibration: %5.3f", calibrate(5)); break; case 68: //"D" //Clamp bee holder clampholder(); break; case 69: //"E" //Unclamp bee holder unclampholder(); break; case 70: //"F" //Open lid liftlid(); break; case 71: //"G" //Close lid closelid(); break; case 72: //"H" //Lock lid locklid(); break; case 73: //"I" //Unlock lid unlocklid(); break; case 74: //"J" //Conduct training cycle trainingcycle(); break; case 75: //"K" //Read air temperature break; case 76: //"L" //Toggle fan on/off fan=!fan; break; case 77: //"M" //Move arm/elbow to basepos armmove(armbasepos, 1); elbowmove(elbowbasepos, 1); break; case 78: //"N" //Move arm/elbow to dippos armmove(armdippos, 1); elbowmove(elbowdippos, 1); break; case 79: //"O" //Move arm/elbow to ticklepos armmove(armticklepos, 1); elbowmove(elbowticklepos, 1); break; case 80: //"P" //Move arm/elbow to feedpos armmove(armfeedpos, 1); elbowmove(elbowfeedpos, 1); break; case 81: //"Q" //Print SWdetect value wait(0.1); swvalue = vswdetect.read(); acucomms.printf("\rVSWDETECT: %5.3f", swvalue); break; case 82: //"R" //multiple calibrations to see variation acucomms.printf("\r\nBee holder: %4u", getserialno()); for (int i=0; i<40; i++) { acucomms.printf("\r\nN%3u C=%5.3f", i+1, calibrate(5)); wait(15); } acucomms.printf("\r\nDone"); break; case 83: //"S" //Toggle air switcher airswitcher = !airswitcher; if (airswitcher) { acucomms.printf("\n\rAirswitcher on"); } else { acucomms.printf("\n\rAirswitcher off"); } break; case 84: //"T" //set date and time on RTC setdatetime(); break; default: //All other characters //Do nothing break; } if (comchar != 88) comchar=0; } acucomms.printf("\n\rSession closed"); //Close logfile fprintf(logfile, "session closed"); fclose(logfile); //close logfile for reading }