git.piffa.net Git - scratch/blob - S4AFirmware16.ino

   1 // NEW IN VERSION 1.6c (by Jorge Gomez):
   2 // Fixed variable type in pin structure: pin.state should be int, not byte
   3 // Optimized speed of execution while receiving data from computer in readSerialPort()
   4
   5 // NEW IN VERSION 1.6b (by Jorge Gomez):
   6 // Added new structure arduinoPins to hold the pins information:
   7 //  - This makes the code easier to read and modify (IMHO)
   8 //  - Allows to change the type of pin more easily to meet non standard use of S4A
   9 //  - Eliminates the need of having to deal with different kind of index access (ie: states[pin-4])
  10 //  - By using an enum to hold all the possible output pin states the code is now more readable
  11 // Changed all functions using old style pin access: configurePins(), resetPins(), readSerialPort(), updateActuator() and sendUpdateActuator()
  12 // Fixed possible overflow every 70 minutes (2e32 us) in pulse() while using micros(). Changed for delayMicroseconds()
  13 // Some minor coding style fixes
  14
  15 // NEW IN VERSION 1.6a  (by Jorge Gomez):
  16 // Fixed compatibility with Arduino Leonardo by avoiding the use of timers
  17 // readSerialPort() optimized:
  18 //  - created state machine for reading the two bytes of the S4A message
  19 //  - updateActuator() is only called if the state is changed
  20 // Memory use optimization
  21 // Cleaning some parts of code
  22 // Avoid using some global variables
  23
  24 // NEW IN VERSION 1.6:
  25 // Refactored reset pins
  26 // Merged code for standard and CR servos
  27 // Merged patch for Leonardo from Peter Mueller (many thanks for this!)
  28
  29 // NEW IN VERSION 1.5:
  30 // Changed pin 8 from standard servo to normal digital output
  31
  32 // NEW IN VERSION 1.4:
  33 // Changed Serial.print() for Serial.write() in ScratchBoardSensorReport function to make it compatible with latest Arduino IDE (1.0)
  34
  35 // NEW IN VERSION 1.3:
  36 // Now it works on GNU/Linux. Also tested with MacOS and Windows 7.
  37 // timer2 set to 20ms, fixing a glitch that made this period unstable in previous versions.
  38 // readSerialport() function optimized.
  39 // pulse() modified so that it receives pulse width as a parameter instead using a global variable.
  40 // updateServoMotors changes its name as a global variable had the same name.
  41 // Some minor fixes.
  42
  43 typedef enum {
  44   input, servomotor, pwm, digital }
  45 pinType;
  46
  47 typedef struct pin {
  48   pinType type;       //Type of pin
  49   int state;         //State of an output
  50   //byte value;       //Value of an input. Not used by now. TODO
  51 };
  52
  53 pin arduinoPins[14];  //Array of struct holding 0-13 pins information
  54
  55 unsigned long lastDataReceivedTime = millis();
  56
  57 void setup()
  58 {
  59   Serial.begin(38400);
  60   Serial.flush();
  61   configurePins();
  62   resetPins();
  63 }
  64
  65 void loop()
  66 {
  67   static unsigned long timerCheckUpdate = millis();
  68
  69   if (millis()-timerCheckUpdate>=20)
  70   {
  71     sendUpdateServomotors();
  72     sendSensorValues();
  73     timerCheckUpdate=millis();
  74   }
  75
  76   readSerialPort();
  77 }
  78
  79 void configurePins()
  80 {
  81   arduinoPins[0].type=input;
  82   arduinoPins[1].type=input;
  83   arduinoPins[2].type=input;
  84   arduinoPins[3].type=input;
  85   arduinoPins[4].type=servomotor;
  86   arduinoPins[5].type=pwm;
  87   arduinoPins[6].type=pwm;
  88   arduinoPins[7].type=servomotor;
  89   arduinoPins[8].type=servomotor;
  90   arduinoPins[9].type=pwm;
  91   arduinoPins[10].type=digital;
  92   arduinoPins[11].type=digital;
  93   arduinoPins[12].type=digital;
  94   arduinoPins[13].type=digital;
  95 }
  96
  97 void resetPins() {
  98   for (byte index=0; index <=13; index++)
  99   {
 100     if (arduinoPins[index].type!=input)
 101     {
 102       pinMode(index, OUTPUT);
 103       if (arduinoPins[index].type==servomotor)
 104       {
 105         arduinoPins[index].state = 255;
 106         servo (index, 255);
 107       }
 108       else
 109       {
 110         arduinoPins[index].state=0;
 111         digitalWrite(index,LOW);
 112       }
 113     }
 114   }
 115 }
 116
 117 void sendSensorValues()
 118 {
 119   unsigned int sensorValues[6], readings[5];
 120   byte sensorIndex;
 121
 122   for (sensorIndex = 0; sensorIndex < 6; sensorIndex++) //for analog sensors, calculate the median of 5 sensor readings in order to avoid variability and power surges
 123   {
 124     for (byte p = 0; p < 5; p++)
 125       readings[p] = analogRead(sensorIndex);
 126     insertionSort(readings, 5); //sort readings
 127     sensorValues[sensorIndex] = readings[2]; //select median reading
 128   }
 129
 130   //send analog sensor values
 131   for (sensorIndex = 0; sensorIndex < 6; sensorIndex++)
 132     ScratchBoardSensorReport(sensorIndex, sensorValues[sensorIndex]);
 133
 134   //send digital sensor values
 135   ScratchBoardSensorReport(6, digitalRead(2)?1023:0);
 136   ScratchBoardSensorReport(7, digitalRead(3)?1023:0);
 137 }
 138
 139 void insertionSort(unsigned int* array, unsigned int n)
 140 {
 141   for (int i = 1; i < n; i++)
 142     for (int j = i; (j > 0) && ( array[j] < array[j-1] ); j--)
 143       swap( array, j, j-1 );
 144 }
 145
 146 void swap(unsigned int* array, unsigned int a, unsigned int b)
 147 {
 148   unsigned int temp = array[a];
 149   array[a] = array[b];
 150   array[b] = temp;
 151 }
 152
 153 void ScratchBoardSensorReport(byte sensor, int value) //PicoBoard protocol, 2 bytes per sensor
 154 {
 155   Serial.write( B10000000
 156     | ((sensor & B1111)<<3)
 157     | ((value>>7) & B111));
 158   Serial.write( value & B1111111);
 159 }
 160
 161 void readSerialPort()
 162 {
 163   byte pin;
 164   int newVal;
 165   static byte actuatorHighByte, actuatorLowByte;
 166   static byte readingSM = 0;
 167
 168   if (Serial.available())
 169   {
 170     if (readingSM == 0)
 171     {
 172       actuatorHighByte = Serial.read();
 173       if (actuatorHighByte >= 128) readingSM = 1;
 174     }
 175     else if (readingSM == 1)
 176     {
 177       actuatorLowByte = Serial.read();
 178       if (actuatorLowByte < 128) readingSM = 2;
 179       else readingSM = 0;
 180     }
 181
 182     if (readingSM == 2)
 183     {
 184       lastDataReceivedTime = millis();
 185       pin = ((actuatorHighByte >> 3) & 0x0F);
 186       newVal = ((actuatorHighByte & 0x07) << 7) | (actuatorLowByte & 0x7F);
 187
 188       if(arduinoPins[pin].state != newVal)
 189       {
 190         arduinoPins[pin].state = newVal;
 191         updateActuator(pin);
 192       }
 193       readingSM = 0;
 194     }
 195   }
 196   else checkScratchDisconnection();
 197 }
 198
 199 void reset() //with xbee module, we need to simulate the setup execution that occurs when a usb connection is opened or closed without this module
 200 {
 201   resetPins();        // reset pins
 202   sendSensorValues(); // protocol handshaking
 203   lastDataReceivedTime = millis();
 204 }
 205
 206 void updateActuator(byte pinNumber)
 207 {
 208   if (arduinoPins[pinNumber].type==digital) digitalWrite(pinNumber, arduinoPins[pinNumber].state);
 209   else if (arduinoPins[pinNumber].type==pwm) analogWrite(pinNumber, arduinoPins[pinNumber].state);
 210 }
 211
 212 void sendUpdateServomotors()
 213 {
 214   for (byte p = 0; p < 10; p++)
 215     if (arduinoPins[p].type == servomotor) servo(p, arduinoPins[p].state);
 216 }
 217
 218 void servo (byte pinNumber, byte angle)
 219 {
 220   if (angle != 255)
 221     pulse(pinNumber, (angle * 10) + 600);
 222 }
 223
 224 void pulse (byte pinNumber, unsigned int pulseWidth)
 225 {
 226   digitalWrite(pinNumber, HIGH);
 227   delayMicroseconds(pulseWidth);
 228   digitalWrite(pinNumber, LOW);
 229 }
 230
 231 void checkScratchDisconnection() //the reset is necessary when using an wireless arduino board (because we need to ensure that arduino isn't waiting the actuators state from Scratch) or when scratch isn't sending information (because is how serial port close is detected)
 232 {
 233   if (millis() - lastDataReceivedTime > 1000) reset(); //reset state if actuators reception timeout = one second
 234 }
 235
 236
 237