Marc Bax / Mbed 2 deprecated Sorbonne2

Sorbonne 2 firmware

main_070.cpp@0:0993f4cf5312, 2011-07-25 (annotated)

Committer:
marcbax
Date:
Mon Jul 25 11:52:02 2011 +0000
Revision:
0:0993f4cf5312
V0.70 as delivered on July 25th

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcbax 0:0993f4cf5312 1 //Firmware to drive Sorbonne-2 bee training module
marcbax 0:0993f4cf5312 2 char* fwversion = "0.70"; //For published versions use format "n.nn"
marcbax 0:0993f4cf5312 3 //For development versions add "D" to end of string
marcbax 0:0993f4cf5312 4 //This version published on 25/07/2011
marcbax 0:0993f4cf5312 5
marcbax 0:0993f4cf5312 6 #include "mbed.h"
marcbax 0:0993f4cf5312 7 #include "Servo.h"
marcbax 0:0993f4cf5312 8
marcbax 0:0993f4cf5312 9 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
marcbax 0:0993f4cf5312 10 DigitalOut lidclose(p5), lidlift(p6);
marcbax 0:0993f4cf5312 11 DigitalOut clamp(p7), unclamp(p8);
marcbax 0:0993f4cf5312 12 DigitalOut lockr(p12), unlockl(p13), lockl(p14), unlockr(p26);
marcbax 0:0993f4cf5312 13 AnalogIn viclamp(p15);
marcbax 0:0993f4cf5312 14 AnalogIn vswdetect(p16);
marcbax 0:0993f4cf5312 15 AnalogIn adc4(p17);
marcbax 0:0993f4cf5312 16 AnalogOut irLED(p18);
marcbax 0:0993f4cf5312 17 AnalogIn vtemp(p19);
marcbax 0:0993f4cf5312 18 AnalogIn ptor(p20);
marcbax 0:0993f4cf5312 19 Servo arm(p21), elbow(p22);
marcbax 0:0993f4cf5312 20 DigitalOut fan(p23);
marcbax 0:0993f4cf5312 21 I2C beeholder(p9, p10);
marcbax 0:0993f4cf5312 22 DigitalOut bhsel(p11);
marcbax 0:0993f4cf5312 23 Serial acucomms(p28, p27);
marcbax 0:0993f4cf5312 24 DigitalIn tmselect(p29);
marcbax 0:0993f4cf5312 25 DigitalOut airswitcher(p25), spare1(p24);
marcbax 0:0993f4cf5312 26 LocalFileSystem local("local"); //allow access to the mbed "thumbdrive" as a file system
marcbax 0:0993f4cf5312 27 //FILE *calfile = fopen("/local/calfile.ini","w"); //file used to store bee holder calibration values
marcbax 0:0993f4cf5312 28 FILE *logfile = fopen("/local/sorb2log.csv","w"); //creates new file sorb2log.csv and set for writing. Overwrites existing file!
marcbax 0:0993f4cf5312 29
marcbax 0:0993f4cf5312 30 //Definition of global constants
marcbax 0:0993f4cf5312 31 //Training module PCB #2 robot arm positions
marcbax 0:0993f4cf5312 32 //Increase in arm position moves arm away from bee holder
marcbax 0:0993f4cf5312 33 //Increase in elbow position lowers tip
marcbax 0:0993f4cf5312 34 float armbasepos = 0.81;
marcbax 0:0993f4cf5312 35 float elbowbasepos = 0.6;
marcbax 0:0993f4cf5312 36 float armdippos = 0.81;
marcbax 0:0993f4cf5312 37 float elbowdippos = 0.84;
marcbax 0:0993f4cf5312 38 float armticklepos = 0.15;
marcbax 0:0993f4cf5312 39 float elbowticklepos = 0.63;
marcbax 0:0993f4cf5312 40 float armfeedpos = 0.08;
marcbax 0:0993f4cf5312 41 float elbowfeedpos = 0.77;
marcbax 0:0993f4cf5312 42
marcbax 0:0993f4cf5312 43 //robot arm hold and move times //all times are in seconds
marcbax 0:0993f4cf5312 44 float diptime = 1; //time that brush stays down dipped in sugar water
marcbax 0:0993f4cf5312 45 float tickletimeout = 3; //maximum time to tickle if no PER is detected
marcbax 0:0993f4cf5312 46 float feedtime = 3; //feeding time
marcbax 0:0993f4cf5312 47
marcbax 0:0993f4cf5312 48 //PER detection parameters
marcbax 0:0993f4cf5312 49 float calsetval = 1.4; //voltage to which phototransistor output will be calibrated
marcbax 0:0993f4cf5312 50 float PERsetval = 2.1; //phototransistor output needs to be above this voltage for PER
marcbax 0:0993f4cf5312 51 float PERtime = 0.2; //phototransistor output needs to stay high this long in seconds
marcbax 0:0993f4cf5312 52
marcbax 0:0993f4cf5312 53 //Protocols
marcbax 0:0993f4cf5312 54 int addr = 0xA2; //I2C address is actually 0x51, but mbed expects left-shifted address
marcbax 0:0993f4cf5312 55 int I2Cfreq = 10000; //I2C bus frequency
marcbax 0:0993f4cf5312 56 int baudrate = 19200; //baudrate of serial connection to ACU
marcbax 0:0993f4cf5312 57
marcbax 0:0993f4cf5312 58 //Definition of global variables
marcbax 0:0993f4cf5312 59
marcbax 0:0993f4cf5312 60 struct holderrecord { //used to hold data on beeholder
marcbax 0:0993f4cf5312 61 int serialno; //beeholder serial number 0-9999
marcbax 0:0993f4cf5312 62 float calvalue; //IR-LED calibration value
marcbax 0:0993f4cf5312 63 char cycleno; //number of cycles gone through in this session
marcbax 0:0993f4cf5312 64 char reslastcycle; //result of last cycle, 1 for PER
marcbax 0:0993f4cf5312 65 char ticklenextcycle; //whether tickling was done for last cycle
marcbax 0:0993f4cf5312 66 time_t tstamp; //timestamp when last cycle was run
marcbax 0:0993f4cf5312 67 };
marcbax 0:0993f4cf5312 68
marcbax 0:0993f4cf5312 69 holderrecord currentholder; //struct record for the current beeholder
marcbax 0:0993f4cf5312 70 holderrecord sessionrecord[101]; //sessionrecord contains holderrecords for up to 100 beeholders
marcbax 0:0993f4cf5312 71
marcbax 0:0993f4cf5312 72 int numbeeholders; //number of beeholders used in current session
marcbax 0:0993f4cf5312 73 int serialnum;
marcbax 0:0993f4cf5312 74 char comchar; //serial command character for module control
marcbax 0:0993f4cf5312 75 float fanspeed;
marcbax 0:0993f4cf5312 76 bool lockstatus, clampstatus, lidstatus; //are true when locked, clamped or closed
marcbax 0:0993f4cf5312 77 char swstatus; //4-bit number to indicate status of 4 detect switches
marcbax 0:0993f4cf5312 78 float swvalue; //ADC value for detect switch voltage
marcbax 0:0993f4cf5312 79
marcbax 0:0993f4cf5312 80 //Function declarations
marcbax 0:0993f4cf5312 81
marcbax 0:0993f4cf5312 82 //Initialises sessionrecords
marcbax 0:0993f4cf5312 83 void sessionrecordinit() {
marcbax 0:0993f4cf5312 84 for (int i=0; i<101; i++) { //set base values for all possible beeholders
marcbax 0:0993f4cf5312 85 sessionrecord[i].serialno = 0; //set serialno to 0 so we can detect a unresponsive holder
marcbax 0:0993f4cf5312 86 sessionrecord[i].calvalue = 0;
marcbax 0:0993f4cf5312 87 sessionrecord[i].cycleno = 0;
marcbax 0:0993f4cf5312 88 sessionrecord[i].reslastcycle = 0;
marcbax 0:0993f4cf5312 89 sessionrecord[i].ticklenextcycle = 1; //default is to tickle on each cycle
marcbax 0:0993f4cf5312 90 }
marcbax 0:0993f4cf5312 91 }
marcbax 0:0993f4cf5312 92
marcbax 0:0993f4cf5312 93 //Reads serial numbers and calibration values from calfile.ini
marcbax 0:0993f4cf5312 94 void getcaldata() {
marcbax 0:0993f4cf5312 95
marcbax 0:0993f4cf5312 96 }
marcbax 0:0993f4cf5312 97
marcbax 0:0993f4cf5312 98 //Initialise at power-up or reset
marcbax 0:0993f4cf5312 99 void init_tm() {
marcbax 0:0993f4cf5312 100 led1=led2=led3=0;
marcbax 0:0993f4cf5312 101 led4=1;
marcbax 0:0993f4cf5312 102 lidclose=lidlift=0;
marcbax 0:0993f4cf5312 103 clamp=unclamp=0;
marcbax 0:0993f4cf5312 104 lockr=unlockr=lockl=unlockl=0;
marcbax 0:0993f4cf5312 105 irLED=0;
marcbax 0:0993f4cf5312 106 bhsel=0;
marcbax 0:0993f4cf5312 107 airswitcher=0;
marcbax 0:0993f4cf5312 108 spare1=0;
marcbax 0:0993f4cf5312 109 fan=0;
marcbax 0:0993f4cf5312 110 acucomms.baud(baudrate);
marcbax 0:0993f4cf5312 111 beeholder.frequency(I2Cfreq);
marcbax 0:0993f4cf5312 112 comchar=0;
marcbax 0:0993f4cf5312 113 lockstatus=0;
marcbax 0:0993f4cf5312 114 numbeeholders=0;
marcbax 0:0993f4cf5312 115 sessionrecordinit();
marcbax 0:0993f4cf5312 116 acucomms.printf("\n\r\rFirmware version: %s", fwversion);
marcbax 0:0993f4cf5312 117 }
marcbax 0:0993f4cf5312 118
marcbax 0:0993f4cf5312 119 //Converts the last 4 digits in the serial number string into a integer 0-9999
marcbax 0:0993f4cf5312 120 int serialstring2int(char bser[8]) {
marcbax 0:0993f4cf5312 121 int tempserial = 0;
marcbax 0:0993f4cf5312 122 tempserial = tempserial + (bser[4]-0x30)*1000; //5th digit is thousands
marcbax 0:0993f4cf5312 123 tempserial = tempserial + (bser[5]-0x30)*100; //6th digit is hundreds
marcbax 0:0993f4cf5312 124 tempserial = tempserial + (bser[6]-0x30)*10; //7th digit is tens
marcbax 0:0993f4cf5312 125 tempserial = tempserial + (bser[7]-0x30); //8th digit is units
marcbax 0:0993f4cf5312 126 return tempserial;
marcbax 0:0993f4cf5312 127 }
marcbax 0:0993f4cf5312 128
marcbax 0:0993f4cf5312 129 //beeholder resets on rising edge of select line
marcbax 0:0993f4cf5312 130 void resetbeeholder() { //need this as mounting beeholder causes undefined start of beeholder firmware
marcbax 0:0993f4cf5312 131 bhsel = 0;
marcbax 0:0993f4cf5312 132 wait(0.1);
marcbax 0:0993f4cf5312 133 bhsel = 1;
marcbax 0:0993f4cf5312 134 wait(0.3);
marcbax 0:0993f4cf5312 135 }
marcbax 0:0993f4cf5312 136
marcbax 0:0993f4cf5312 137 //gets holderrecord index for the holder with a certain serial number
marcbax 0:0993f4cf5312 138 int getholderindex(int serialno) {
marcbax 0:0993f4cf5312 139 for (int i=1; i<=100; i++) { //reverse lookup of index for a certain serial number
marcbax 0:0993f4cf5312 140 if (sessionrecord[i].serialno == serialno) return i;
marcbax 0:0993f4cf5312 141 }
marcbax 0:0993f4cf5312 142 return 0; //if the record is not found, returns i==0
marcbax 0:0993f4cf5312 143 }
marcbax 0:0993f4cf5312 144
marcbax 0:0993f4cf5312 145 //Reads beeholder serial number
marcbax 0:0993f4cf5312 146 int getserialno() {
marcbax 0:0993f4cf5312 147 char bser[8]; //define 8-byte serial number string to read
marcbax 0:0993f4cf5312 148 resetbeeholder(); //does not work without this!!
marcbax 0:0993f4cf5312 149 for (int i=0;i<8;i++) bser[i]=0x30;
marcbax 0:0993f4cf5312 150 bhsel = 0; //pull select line low
marcbax 0:0993f4cf5312 151 wait(0.2);
marcbax 0:0993f4cf5312 152 beeholder.stop(); //I2C stop condition
marcbax 0:0993f4cf5312 153 wait(0.2); //delay for beeholder to respond
marcbax 0:0993f4cf5312 154 beeholder.write(addr,0x0,1); //initial write before read
marcbax 0:0993f4cf5312 155 beeholder.read(addr,bser,8); //read 8 byte serial number
marcbax 0:0993f4cf5312 156 bhsel = 1; //pull select line high
marcbax 0:0993f4cf5312 157 int serialno = serialstring2int(bser); //translate serial number string to integer
marcbax 0:0993f4cf5312 158 return serialno;
marcbax 0:0993f4cf5312 159 }
marcbax 0:0993f4cf5312 160
marcbax 0:0993f4cf5312 161 //Returns 1 if a PER is detected within timeout seconds
marcbax 0:0993f4cf5312 162 int detectPER(float timeout) {
marcbax 0:0993f4cf5312 163 Timer ttotal, tper;
marcbax 0:0993f4cf5312 164 ttotal.start(); //start timers for time-out and PER-detect
marcbax 0:0993f4cf5312 165 ttotal.reset();
marcbax 0:0993f4cf5312 166 tper.start();
marcbax 0:0993f4cf5312 167 tper.reset();
marcbax 0:0993f4cf5312 168 while ((ttotal.read() < timeout) && (tper.read() < PERtime)) { //loop until timeout or PER detected
marcbax 0:0993f4cf5312 169 wait_ms(10);
marcbax 0:0993f4cf5312 170 if (ptor * 3.3 < PERsetval) tper.reset(); //if phototransistor voltage below treshold keep PER timer in reset
marcbax 0:0993f4cf5312 171 //if above treshold let timer run until it reaches PERtime
marcbax 0:0993f4cf5312 172 }
marcbax 0:0993f4cf5312 173 ttotal.stop();
marcbax 0:0993f4cf5312 174 tper.stop();
marcbax 0:0993f4cf5312 175 return (tper.read() >= PERtime); //if the loop exit condition was a PER, return a TRUE value
marcbax 0:0993f4cf5312 176 }
marcbax 0:0993f4cf5312 177
marcbax 0:0993f4cf5312 178 //Function performs beeholder/IR-led calibration
marcbax 0:0993f4cf5312 179 float calibrate(int runs) {
marcbax 0:0993f4cf5312 180 float tempcal, ptorhold;
marcbax 0:0993f4cf5312 181 tempcal=0.5; //start calibration at 50% voltage
marcbax 0:0993f4cf5312 182 irLED=tempcal;
marcbax 0:0993f4cf5312 183 float calstep; //start calstep at 25% voltage
marcbax 0:0993f4cf5312 184 calstep = tempcal/2;
marcbax 0:0993f4cf5312 185 //acucomms.printf("\n\rInitial calibration:");
marcbax 0:0993f4cf5312 186 for (int i=0; i<10; i++) { //does a "10-bit binary search" for the correct voltage to get a good response
marcbax 0:0993f4cf5312 187 irLED = tempcal;
marcbax 0:0993f4cf5312 188 wait(0.1); //important to allow AD converter to settle
marcbax 0:0993f4cf5312 189 ptorhold=ptor;
marcbax 0:0993f4cf5312 190 //acucomms.printf("\n\r%5.3f - %5.3f", tempcal, ptorhold);
marcbax 0:0993f4cf5312 191 if (ptorhold < calsetval/3.3) { //check phototransistor voltage against desired value
marcbax 0:0993f4cf5312 192 tempcal = tempcal - calstep; //if phototransistor voltage is too low then reduce brightness
marcbax 0:0993f4cf5312 193 }
marcbax 0:0993f4cf5312 194 else {
marcbax 0:0993f4cf5312 195 tempcal = tempcal + calstep; //if phototransistor voltage is too high then increase brightness
marcbax 0:0993f4cf5312 196 }
marcbax 0:0993f4cf5312 197 calstep = calstep/2; //on each loop of the for-cycle make smaller changes to IR LED voltage
marcbax 0:0993f4cf5312 198 }
marcbax 0:0993f4cf5312 199 float calib;
marcbax 0:0993f4cf5312 200 calib = tempcal; //set preliminary calibration to the value just measured
marcbax 0:0993f4cf5312 201 for (int j=1; j<runs; j++) { //run another j-1 runs, this corrects for antennae-movement as
marcbax 0:0993f4cf5312 202 tempcal=0.5; //we use the lowest calibration value from j runs
marcbax 0:0993f4cf5312 203 irLED=tempcal; //this is similar to what we do in the cassettes
marcbax 0:0993f4cf5312 204 calstep = tempcal/2;
marcbax 0:0993f4cf5312 205 for (int i=0;i<10;i++) {
marcbax 0:0993f4cf5312 206 irLED = tempcal;
marcbax 0:0993f4cf5312 207 wait(0.1);
marcbax 0:0993f4cf5312 208 ptorhold=ptor;
marcbax 0:0993f4cf5312 209 if (ptorhold < calsetval/3.3) {
marcbax 0:0993f4cf5312 210 tempcal = tempcal - calstep;
marcbax 0:0993f4cf5312 211 }
marcbax 0:0993f4cf5312 212 else {
marcbax 0:0993f4cf5312 213 tempcal = tempcal + calstep;
marcbax 0:0993f4cf5312 214 }
marcbax 0:0993f4cf5312 215 calstep = calstep/2;
marcbax 0:0993f4cf5312 216 }
marcbax 0:0993f4cf5312 217 if (tempcal < calib) calib = tempcal; //use the lowest of j calibration values
marcbax 0:0993f4cf5312 218 }
marcbax 0:0993f4cf5312 219 irLED = 0;
marcbax 0:0993f4cf5312 220 return calib;
marcbax 0:0993f4cf5312 221 }
marcbax 0:0993f4cf5312 222
marcbax 0:0993f4cf5312 223 //switches the IR LED on at the right brightness level for the beeholder
marcbax 0:0993f4cf5312 224 void IRledON(int i) {
marcbax 0:0993f4cf5312 225 irLED = sessionrecord[i].calvalue;
marcbax 0:0993f4cf5312 226 }
marcbax 0:0993f4cf5312 227
marcbax 0:0993f4cf5312 228 //moves arm to a position in a specified time, allowing speed control
marcbax 0:0993f4cf5312 229 void armmove (float endpos, float movetime) {
marcbax 0:0993f4cf5312 230 float startpos = arm.read();
marcbax 0:0993f4cf5312 231 int numsteps = (movetime * 50); //50 pulses/second, so each step is 1 servo pulse
marcbax 0:0993f4cf5312 232 for (int i=0; i<numsteps; i++) {
marcbax 0:0993f4cf5312 233 arm = startpos + i * (endpos - startpos)/numsteps;
marcbax 0:0993f4cf5312 234 wait(movetime/numsteps);
marcbax 0:0993f4cf5312 235 }
marcbax 0:0993f4cf5312 236 arm = endpos;
marcbax 0:0993f4cf5312 237 }
marcbax 0:0993f4cf5312 238
marcbax 0:0993f4cf5312 239 //moves elbow to a position in a specified time, allowing speed control
marcbax 0:0993f4cf5312 240 void elbowmove (float endpos, float movetime) {
marcbax 0:0993f4cf5312 241 float startpos = elbow.read();
marcbax 0:0993f4cf5312 242 int numsteps = (movetime * 50); //50 pulses/second, so each step is 1 servo pulse
marcbax 0:0993f4cf5312 243 for (int i=0; i<numsteps; i++) {
marcbax 0:0993f4cf5312 244 elbow = startpos + i * (endpos - startpos)/numsteps;
marcbax 0:0993f4cf5312 245 wait(movetime/numsteps);
marcbax 0:0993f4cf5312 246 }
marcbax 0:0993f4cf5312 247 elbow = endpos;
marcbax 0:0993f4cf5312 248 }
marcbax 0:0993f4cf5312 249
marcbax 0:0993f4cf5312 250 //Performs a conditioning cycle. if tickle==0, no tickling takes place. Return==1 if a PER has been detected
marcbax 0:0993f4cf5312 251 int condcycle(int tickle) {
marcbax 0:0993f4cf5312 252 int perseen;
marcbax 0:0993f4cf5312 253 perseen = 0;
marcbax 0:0993f4cf5312 254 //dip brush
marcbax 0:0993f4cf5312 255 armmove(armdippos, 0.5);
marcbax 0:0993f4cf5312 256 elbowmove(elbowdippos, 0.3);
marcbax 0:0993f4cf5312 257 wait(diptime);
marcbax 0:0993f4cf5312 258 elbowmove(elbowbasepos, 0.3);
marcbax 0:0993f4cf5312 259 armmove(armbasepos, 0.5);
marcbax 0:0993f4cf5312 260 airswitcher=1; //switch air to target odour
marcbax 0:0993f4cf5312 261 //tickle
marcbax 0:0993f4cf5312 262 if (tickle) { //if tickling, first tickle then wait for PER or timeout
marcbax 0:0993f4cf5312 263 elbowmove(elbowticklepos, 0.2);
marcbax 0:0993f4cf5312 264 armmove(armticklepos, 1.5); //slower move of arm towards bee
marcbax 0:0993f4cf5312 265 perseen = detectPER(tickletimeout); //tickle until timeout or PER detected
marcbax 0:0993f4cf5312 266 }
marcbax 0:0993f4cf5312 267 //or not tickle
marcbax 0:0993f4cf5312 268 else {
marcbax 0:0993f4cf5312 269 perseen = detectPER(tickletimeout); //if not tickling, wait for PER or timeout then move to pre-feeding position
marcbax 0:0993f4cf5312 270 armmove(armticklepos-0.05, 1); //move to position between LED and holder
marcbax 0:0993f4cf5312 271 elbowmove(elbowticklepos, 0.3);
marcbax 0:0993f4cf5312 272 }
marcbax 0:0993f4cf5312 273 //feeding only if you have tickled or a PER has been detected
marcbax 0:0993f4cf5312 274 if (tickle || perseen) { //only feed if a PER has been detector, or "tickle" is true
marcbax 0:0993f4cf5312 275 elbowmove(elbowfeedpos, 0.3);
marcbax 0:0993f4cf5312 276 armmove(armfeedpos, 0.3);
marcbax 0:0993f4cf5312 277 wait(feedtime);
marcbax 0:0993f4cf5312 278 armmove(armticklepos -0.05, 0.3);
marcbax 0:0993f4cf5312 279 elbowmove(elbowticklepos, 0.3);
marcbax 0:0993f4cf5312 280 }
marcbax 0:0993f4cf5312 281 //move back to base position
marcbax 0:0993f4cf5312 282 airswitcher=0; //switch back to clean air
marcbax 0:0993f4cf5312 283 armmove(armbasepos, 0.5); //back to basepos
marcbax 0:0993f4cf5312 284 elbowmove(elbowbasepos, 0.3); //
marcbax 0:0993f4cf5312 285 return perseen;
marcbax 0:0993f4cf5312 286 }
marcbax 0:0993f4cf5312 287
marcbax 0:0993f4cf5312 288
marcbax 0:0993f4cf5312 289 char switchstatus(float vswdetect) {
marcbax 0:0993f4cf5312 290 //outputs char (4-bit flag) to reflect positions of 4 detect switches
marcbax 0:0993f4cf5312 291 if (vswdetect>0.38 && vswdetect<0.4) return 11;
marcbax 0:0993f4cf5312 292 if (vswdetect>0.4 && vswdetect<0.45) return 9;
marcbax 0:0993f4cf5312 293 if (vswdetect>0.45 && vswdetect<0.49) return 10;
marcbax 0:0993f4cf5312 294 if (vswdetect>0.49 && vswdetect<0.54) return 8;
marcbax 0:0993f4cf5312 295 if (vswdetect>0.54 && vswdetect<0.58) return 7;
marcbax 0:0993f4cf5312 296 if (vswdetect>0.58 && vswdetect<0.62) return 5;
marcbax 0:0993f4cf5312 297 if (vswdetect>0.62 && vswdetect<0.66) return 3;
marcbax 0:0993f4cf5312 298 if (vswdetect>0.66 && vswdetect<0.74) return 1;
marcbax 0:0993f4cf5312 299 if (vswdetect>0.74 && vswdetect<0.8) return 6;
marcbax 0:0993f4cf5312 300 if (vswdetect>0.8 && vswdetect<0.85) return 4;
marcbax 0:0993f4cf5312 301 if (vswdetect>0.85 && vswdetect<0.95) return 2;
marcbax 0:0993f4cf5312 302 if (vswdetect>0.95 && vswdetect<1.01) return 0;
marcbax 0:0993f4cf5312 303 return 16;
marcbax 0:0993f4cf5312 304 }
marcbax 0:0993f4cf5312 305
marcbax 0:0993f4cf5312 306 //Clamp bee holder
marcbax 0:0993f4cf5312 307 void clampholder() {
marcbax 0:0993f4cf5312 308 unclamp=1;
marcbax 0:0993f4cf5312 309 wait(4);
marcbax 0:0993f4cf5312 310 unclamp=0;
marcbax 0:0993f4cf5312 311 }
marcbax 0:0993f4cf5312 312
marcbax 0:0993f4cf5312 313 //Unclamp bee holder
marcbax 0:0993f4cf5312 314 void unclampholder() {
marcbax 0:0993f4cf5312 315 clamp=1;
marcbax 0:0993f4cf5312 316 wait(1);
marcbax 0:0993f4cf5312 317 clamp=0;
marcbax 0:0993f4cf5312 318 }
marcbax 0:0993f4cf5312 319
marcbax 0:0993f4cf5312 320 //Lift lid
marcbax 0:0993f4cf5312 321 void liftlid() {
marcbax 0:0993f4cf5312 322 bool notup;
marcbax 0:0993f4cf5312 323 notup=1;
marcbax 0:0993f4cf5312 324 lidclose=1;
marcbax 0:0993f4cf5312 325 while (notup) {
marcbax 0:0993f4cf5312 326 wait(0.01);
marcbax 0:0993f4cf5312 327 notup = (switchstatus(vswdetect.read()) != 7);
marcbax 0:0993f4cf5312 328 }
marcbax 0:0993f4cf5312 329 lidclose=0;
marcbax 0:0993f4cf5312 330 lidstatus=0;
marcbax 0:0993f4cf5312 331 }
marcbax 0:0993f4cf5312 332
marcbax 0:0993f4cf5312 333 //Close lid
marcbax 0:0993f4cf5312 334 void closelid() {
marcbax 0:0993f4cf5312 335 bool notdown;
marcbax 0:0993f4cf5312 336 notdown=1;
marcbax 0:0993f4cf5312 337 lidlift=1;
marcbax 0:0993f4cf5312 338 while (notdown) {
marcbax 0:0993f4cf5312 339 wait(0.01);
marcbax 0:0993f4cf5312 340 notdown = (switchstatus(vswdetect.read()) != 11);
marcbax 0:0993f4cf5312 341 }
marcbax 0:0993f4cf5312 342 lidlift=0;
marcbax 0:0993f4cf5312 343 lidstatus=1;
marcbax 0:0993f4cf5312 344 }
marcbax 0:0993f4cf5312 345
marcbax 0:0993f4cf5312 346 //Lock lid
marcbax 0:0993f4cf5312 347 void locklid() {
marcbax 0:0993f4cf5312 348 wait(0.1);
marcbax 0:0993f4cf5312 349 }
marcbax 0:0993f4cf5312 350
marcbax 0:0993f4cf5312 351 //Unlock lid
marcbax 0:0993f4cf5312 352 void unlocklid() {
marcbax 0:0993f4cf5312 353 wait(0.1);
marcbax 0:0993f4cf5312 354 }
marcbax 0:0993f4cf5312 355
marcbax 0:0993f4cf5312 356 //Set date and time
marcbax 0:0993f4cf5312 357 void setdatetime() {
marcbax 0:0993f4cf5312 358
marcbax 0:0993f4cf5312 359
marcbax 0:0993f4cf5312 360 }
marcbax 0:0993f4cf5312 361
marcbax 0:0993f4cf5312 362
marcbax 0:0993f4cf5312 363 void registerbeeholder() {
marcbax 0:0993f4cf5312 364 //registers and calibrates the bee holder currently clamped
marcbax 0:0993f4cf5312 365 int serialno, i;
marcbax 0:0993f4cf5312 366 float calvalue;
marcbax 0:0993f4cf5312 367 serialno = getserialno();
marcbax 0:0993f4cf5312 368 i = getholderindex(serialno);
marcbax 0:0993f4cf5312 369 if (i == 0) {
marcbax 0:0993f4cf5312 370 numbeeholders++;
marcbax 0:0993f4cf5312 371 if (numbeeholders == 101) {
marcbax 0:0993f4cf5312 372 acucomms.printf("Number of holders exceeds 100");
marcbax 0:0993f4cf5312 373 }
marcbax 0:0993f4cf5312 374 else {
marcbax 0:0993f4cf5312 375 sessionrecord[numbeeholders].serialno = serialno;
marcbax 0:0993f4cf5312 376 calvalue = calibrate(5);
marcbax 0:0993f4cf5312 377 if (calvalue < 0.98 && calvalue > 0.25) {
marcbax 0:0993f4cf5312 378 sessionrecord[numbeeholders].calvalue = calvalue;
marcbax 0:0993f4cf5312 379 acucomms.printf("\n\rCal %4u - %4.2fV", serialno, calvalue*3.3);
marcbax 0:0993f4cf5312 380 }
marcbax 0:0993f4cf5312 381 else {
marcbax 0:0993f4cf5312 382 acucomms.printf("\n\rCal %4u - invalid", serialno);
marcbax 0:0993f4cf5312 383 }
marcbax 0:0993f4cf5312 384 }
marcbax 0:0993f4cf5312 385 }
marcbax 0:0993f4cf5312 386 }
marcbax 0:0993f4cf5312 387
marcbax 0:0993f4cf5312 388 /*
marcbax 0:0993f4cf5312 389 //Registers and calibrates all beeholders used in this session
marcbax 0:0993f4cf5312 390 int registerbeeholders() {
marcbax 0:0993f4cf5312 391 int i;
marcbax 0:0993f4cf5312 392 bool done;
marcbax 0:0993f4cf5312 393 char buffert[30];
marcbax 0:0993f4cf5312 394 i = done = 0;
marcbax 0:0993f4cf5312 395 //cleardisplay(); //clear screen and home cursor
marcbax 0:0993f4cf5312 396 fprintf(logfile, "calibration record:\r");
marcbax 0:0993f4cf5312 397 fprintf(logfile, "i, serialno, LED V, time\r");
marcbax 0:0993f4cf5312 398 while (i<30 && !done) { //register and calibrate a maximum of 30 beeholders
marcbax 0:0993f4cf5312 399 //display.printf("calibrating %u\r",i+1);
marcbax 0:0993f4cf5312 400 sessionrecord[i].serialno = getserialno(); //read serial number
marcbax 0:0993f4cf5312 401 if (sessionrecord[i].serialno != 0) { //check if serial number correctly read - if not it will be 0000
marcbax 0:0993f4cf5312 402 sessionrecord[i].calvalue = calibrate(station, 5); //5 calibration cycles
marcbax 0:0993f4cf5312 403 if ((sessionrecord[i].calvalue > 0.25) && (sessionrecord[i].calvalue < 0.97)) { //check that calvalue is in expected range
marcbax 0:0993f4cf5312 404 sessionrecord[i].tstamp = time(NULL); //create timestamp NOW
marcbax 0:0993f4cf5312 405 strftime(buffert, 20, "%X", localtime(&sessionrecord[i].tstamp)); //formats time part of timestamp
marcbax 0:0993f4cf5312 406 cleardisplay();
marcbax 0:0993f4cf5312 407 display.printf("SN %4u - cal %4.2fV\r", sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3);
marcbax 0:0993f4cf5312 408 display.printf("OK for next\r");
marcbax 0:0993f4cf5312 409 display.printf("DOWN for training");
marcbax 0:0993f4cf5312 410 fprintf(logfile, "%4u,%6u,%6.2f, %s, calibrated\r", i, sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3, buffert);
marcbax 0:0993f4cf5312 411 i++;
marcbax 0:0993f4cf5312 412 }
marcbax 0:0993f4cf5312 413 else {
marcbax 0:0993f4cf5312 414 cleardisplay();
marcbax 0:0993f4cf5312 415 display.printf("SN %4u - cal %4.2fV\r", sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3);
marcbax 0:0993f4cf5312 416 display.printf("Cal out of range!\r");
marcbax 0:0993f4cf5312 417 multibeeps(2,0.5);
marcbax 0:0993f4cf5312 418 display.printf("OK to recalibrate\r");
marcbax 0:0993f4cf5312 419 fprintf(logfile, "%4u,%6u,%6.2f, %s, out of range\r", i, sessionrecord[i].serialno, sessionrecord[i].calvalue*3.3, buffert);
marcbax 0:0993f4cf5312 420 }
marcbax 0:0993f4cf5312 421 while (!done && oksw) { //loop until OK or DOWN are pressed
marcbax 0:0993f4cf5312 422 wait(0.02);
marcbax 0:0993f4cf5312 423 done = !dnsw; //DOWN exits registration cycle and moves to training
marcbax 0:0993f4cf5312 424 }
marcbax 0:0993f4cf5312 425 }
marcbax 0:0993f4cf5312 426 else { //retry when serialno can't be read (is 0000)
marcbax 0:0993f4cf5312 427 cleardisplay();
marcbax 0:0993f4cf5312 428 multibeeps(3,0.3); //beep-beep-beep when beeholder not correctly read
marcbax 0:0993f4cf5312 429 display.printf("invalid serial no\r");
marcbax 0:0993f4cf5312 430 display.printf("reseat beeholder\r");
marcbax 0:0993f4cf5312 431 display.printf("press OK");
marcbax 0:0993f4cf5312 432 while (oksw) wait(0.02); //loop until OK is pressed to start calibration loop again
marcbax 0:0993f4cf5312 433 }
marcbax 0:0993f4cf5312 434 cleardisplay();
marcbax 0:0993f4cf5312 435 }
marcbax 0:0993f4cf5312 436 return i; //upon done condition, i== number of beeholders calibrated
marcbax 0:0993f4cf5312 437 }
marcbax 0:0993f4cf5312 438
marcbax 0:0993f4cf5312 439 */
marcbax 0:0993f4cf5312 440
marcbax 0:0993f4cf5312 441 float calcairtemp(float vntc) {
marcbax 0:0993f4cf5312 442 return 0.1;
marcbax 0:0993f4cf5312 443 }
marcbax 0:0993f4cf5312 444
marcbax 0:0993f4cf5312 445 //all the elements making up a single training cycle with one beeholder
marcbax 0:0993f4cf5312 446 void trainingcycle() {
marcbax 0:0993f4cf5312 447 wait(0.2);
marcbax 0:0993f4cf5312 448 time_t tstamp = time(NULL); //create timestamp NOW
marcbax 0:0993f4cf5312 449 char buffert[30];
marcbax 0:0993f4cf5312 450 strftime(buffert, 20, "%X", localtime(&tstamp)); //format timestamp to time string
marcbax 0:0993f4cf5312 451 int serialno;
marcbax 0:0993f4cf5312 452 serialno = getserialno();
marcbax 0:0993f4cf5312 453 int i = getholderindex(serialno); //get index i for serial number
marcbax 0:0993f4cf5312 454 if (i != 0) {
marcbax 0:0993f4cf5312 455 IRledON(i); //switch IR LED on at correct brightness
marcbax 0:0993f4cf5312 456 sessionrecord[i].cycleno++; //increment cycle number for this beeholder
marcbax 0:0993f4cf5312 457 acucomms.printf("\n\rSN: %4u, cycle %u - ", serialno, sessionrecord[i].cycleno);
marcbax 0:0993f4cf5312 458 sessionrecord[i].reslastcycle = condcycle(sessionrecord[i].ticklenextcycle); //do a conditioning cycle
marcbax 0:0993f4cf5312 459 fprintf(logfile, "%s,",buffert);
marcbax 0:0993f4cf5312 460 fprintf(logfile, " %4u,",serialno);
marcbax 0:0993f4cf5312 461 fprintf(logfile, " %2u,", sessionrecord[i].cycleno);
marcbax 0:0993f4cf5312 462 if (sessionrecord[i].reslastcycle) { //log PER or TimeOut
marcbax 0:0993f4cf5312 463 fprintf(logfile, " PER,");
marcbax 0:0993f4cf5312 464 }
marcbax 0:0993f4cf5312 465 else {
marcbax 0:0993f4cf5312 466 fprintf(logfile, " TO,");
marcbax 0:0993f4cf5312 467 }
marcbax 0:0993f4cf5312 468 fprintf(logfile, " training\r");
marcbax 0:0993f4cf5312 469 if (sessionrecord[i].reslastcycle) {
marcbax 0:0993f4cf5312 470 acucomms.printf("PER detected");
marcbax 0:0993f4cf5312 471 }
marcbax 0:0993f4cf5312 472 else {
marcbax 0:0993f4cf5312 473 acucomms.printf("PER time-out");
marcbax 0:0993f4cf5312 474 }
marcbax 0:0993f4cf5312 475 }
marcbax 0:0993f4cf5312 476 else {
marcbax 0:0993f4cf5312 477 acucomms.printf("\n\rBee holder %4u not registered", serialno);
marcbax 0:0993f4cf5312 478 }
marcbax 0:0993f4cf5312 479 }
marcbax 0:0993f4cf5312 480
marcbax 0:0993f4cf5312 481
marcbax 0:0993f4cf5312 482
marcbax 0:0993f4cf5312 483 int main() {
marcbax 0:0993f4cf5312 484 init_tm();
marcbax 0:0993f4cf5312 485 while (comchar != 88) {
marcbax 0:0993f4cf5312 486 comchar = acucomms.getc();
marcbax 0:0993f4cf5312 487 switch (comchar) {
marcbax 0:0993f4cf5312 488 case 65: //"A"
marcbax 0:0993f4cf5312 489 //Continue after pause
marcbax 0:0993f4cf5312 490 break;
marcbax 0:0993f4cf5312 491 case 66: //"B"
marcbax 0:0993f4cf5312 492 //Register bee holder
marcbax 0:0993f4cf5312 493 registerbeeholder();
marcbax 0:0993f4cf5312 494 break;
marcbax 0:0993f4cf5312 495 case 67: //"C"
marcbax 0:0993f4cf5312 496 //Calibrate bee holder
marcbax 0:0993f4cf5312 497 acucomms.printf("\r\nCalibration: %5.3f", calibrate(5));
marcbax 0:0993f4cf5312 498 break;
marcbax 0:0993f4cf5312 499 case 68: //"D"
marcbax 0:0993f4cf5312 500 //Clamp bee holder
marcbax 0:0993f4cf5312 501 clampholder();
marcbax 0:0993f4cf5312 502 break;
marcbax 0:0993f4cf5312 503 case 69: //"E"
marcbax 0:0993f4cf5312 504 //Unclamp bee holder
marcbax 0:0993f4cf5312 505 unclampholder();
marcbax 0:0993f4cf5312 506 break;
marcbax 0:0993f4cf5312 507 case 70: //"F"
marcbax 0:0993f4cf5312 508 //Open lid
marcbax 0:0993f4cf5312 509 liftlid();
marcbax 0:0993f4cf5312 510 break;
marcbax 0:0993f4cf5312 511 case 71: //"G"
marcbax 0:0993f4cf5312 512 //Close lid
marcbax 0:0993f4cf5312 513 closelid();
marcbax 0:0993f4cf5312 514 break;
marcbax 0:0993f4cf5312 515 case 72: //"H"
marcbax 0:0993f4cf5312 516 //Lock lid
marcbax 0:0993f4cf5312 517 locklid();
marcbax 0:0993f4cf5312 518 break;
marcbax 0:0993f4cf5312 519 case 73: //"I"
marcbax 0:0993f4cf5312 520 //Unlock lid
marcbax 0:0993f4cf5312 521 unlocklid();
marcbax 0:0993f4cf5312 522 break;
marcbax 0:0993f4cf5312 523 case 74: //"J"
marcbax 0:0993f4cf5312 524 //Conduct training cycle
marcbax 0:0993f4cf5312 525 trainingcycle();
marcbax 0:0993f4cf5312 526 break;
marcbax 0:0993f4cf5312 527 case 75: //"K"
marcbax 0:0993f4cf5312 528 //Read air temperature
marcbax 0:0993f4cf5312 529 break;
marcbax 0:0993f4cf5312 530 case 76: //"L"
marcbax 0:0993f4cf5312 531 //Toggle fan on/off
marcbax 0:0993f4cf5312 532 fan=!fan;
marcbax 0:0993f4cf5312 533 break;
marcbax 0:0993f4cf5312 534 case 77: //"M"
marcbax 0:0993f4cf5312 535 //Move arm/elbow to basepos
marcbax 0:0993f4cf5312 536 armmove(armbasepos, 1);
marcbax 0:0993f4cf5312 537 elbowmove(elbowbasepos, 1);
marcbax 0:0993f4cf5312 538 break;
marcbax 0:0993f4cf5312 539 case 78: //"N"
marcbax 0:0993f4cf5312 540 //Move arm/elbow to dippos
marcbax 0:0993f4cf5312 541 armmove(armdippos, 1);
marcbax 0:0993f4cf5312 542 elbowmove(elbowdippos, 1);
marcbax 0:0993f4cf5312 543 break;
marcbax 0:0993f4cf5312 544 case 79: //"O"
marcbax 0:0993f4cf5312 545 //Move arm/elbow to ticklepos
marcbax 0:0993f4cf5312 546 armmove(armticklepos, 1);
marcbax 0:0993f4cf5312 547 elbowmove(elbowticklepos, 1);
marcbax 0:0993f4cf5312 548 break;
marcbax 0:0993f4cf5312 549 case 80: //"P"
marcbax 0:0993f4cf5312 550 //Move arm/elbow to feedpos
marcbax 0:0993f4cf5312 551 armmove(armfeedpos, 1);
marcbax 0:0993f4cf5312 552 elbowmove(elbowfeedpos, 1);
marcbax 0:0993f4cf5312 553 break;
marcbax 0:0993f4cf5312 554 case 81: //"Q"
marcbax 0:0993f4cf5312 555 //Print SWdetect value
marcbax 0:0993f4cf5312 556 wait(0.1);
marcbax 0:0993f4cf5312 557 swvalue = vswdetect.read();
marcbax 0:0993f4cf5312 558 acucomms.printf("\rVSWDETECT: %5.3f", swvalue);
marcbax 0:0993f4cf5312 559 break;
marcbax 0:0993f4cf5312 560 case 82: //"R"
marcbax 0:0993f4cf5312 561 //multiple calibrations to see variation
marcbax 0:0993f4cf5312 562 acucomms.printf("\r\nBee holder: %4u", getserialno());
marcbax 0:0993f4cf5312 563 for (int i=0; i<40; i++) {
marcbax 0:0993f4cf5312 564 acucomms.printf("\r\nN%3u C=%5.3f", i+1, calibrate(5));
marcbax 0:0993f4cf5312 565 wait(15);
marcbax 0:0993f4cf5312 566 }
marcbax 0:0993f4cf5312 567 acucomms.printf("\r\nDone");
marcbax 0:0993f4cf5312 568 break;
marcbax 0:0993f4cf5312 569 case 83: //"S"
marcbax 0:0993f4cf5312 570 //Toggle air switcher
marcbax 0:0993f4cf5312 571 airswitcher = !airswitcher;
marcbax 0:0993f4cf5312 572 if (airswitcher) {
marcbax 0:0993f4cf5312 573 acucomms.printf("\n\rAirswitcher on");
marcbax 0:0993f4cf5312 574 }
marcbax 0:0993f4cf5312 575 else {
marcbax 0:0993f4cf5312 576 acucomms.printf("\n\rAirswitcher off");
marcbax 0:0993f4cf5312 577 }
marcbax 0:0993f4cf5312 578 break;
marcbax 0:0993f4cf5312 579 case 84: //"T"
marcbax 0:0993f4cf5312 580 //set date and time on RTC
marcbax 0:0993f4cf5312 581 setdatetime();
marcbax 0:0993f4cf5312 582 break;
marcbax 0:0993f4cf5312 583 default: //All other characters
marcbax 0:0993f4cf5312 584 //Do nothing
marcbax 0:0993f4cf5312 585 break;
marcbax 0:0993f4cf5312 586 }
marcbax 0:0993f4cf5312 587 if (comchar != 88) comchar=0;
marcbax 0:0993f4cf5312 588 }
marcbax 0:0993f4cf5312 589 acucomms.printf("\n\rSession closed");
marcbax 0:0993f4cf5312 590 //Close logfile
marcbax 0:0993f4cf5312 591 fprintf(logfile, "session closed");
marcbax 0:0993f4cf5312 592 fclose(logfile); //close logfile for reading
marcbax 0:0993f4cf5312 593 }