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    
}