Marc Bax
Sorbonne 2 firmware
Diff: main_070.cpp
- Revision:
- 0:0993f4cf5312
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main_070.cpp Mon Jul 25 11:52:02 2011 +0000
@@ -0,0 +1,593 @@
+//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
+}