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()
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
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
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!)
29 // NEW IN VERSION 1.5:
30 // Changed pin 8 from standard servo to normal digital output
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)
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.
44 input, servomotor, pwm, digital }
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
53 pin arduinoPins[14]; //Array of struct holding 0-13 pins information
55 unsigned long lastDataReceivedTime = millis();
67 static unsigned long timerCheckUpdate = millis();
69 if (millis()-timerCheckUpdate>=20)
71 sendUpdateServomotors();
73 timerCheckUpdate=millis();
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;
98 for (byte index=0; index <=13; index++)
100 if (arduinoPins[index].type!=input)
102 pinMode(index, OUTPUT);
103 if (arduinoPins[index].type==servomotor)
105 arduinoPins[index].state = 255;
110 arduinoPins[index].state=0;
111 digitalWrite(index,LOW);
117 void sendSensorValues()
119 unsigned int sensorValues[6], readings[5];
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
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
130 //send analog sensor values
131 for (sensorIndex = 0; sensorIndex < 6; sensorIndex++)
132 ScratchBoardSensorReport(sensorIndex, sensorValues[sensorIndex]);
134 //send digital sensor values
135 ScratchBoardSensorReport(6, digitalRead(2)?1023:0);
136 ScratchBoardSensorReport(7, digitalRead(3)?1023:0);
139 void insertionSort(unsigned int* array, unsigned int n)
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 );
146 void swap(unsigned int* array, unsigned int a, unsigned int b)
148 unsigned int temp = array[a];
153 void ScratchBoardSensorReport(byte sensor, int value) //PicoBoard protocol, 2 bytes per sensor
155 Serial.write( B10000000
156 | ((sensor & B1111)<<3)
157 | ((value>>7) & B111));
158 Serial.write( value & B1111111);
161 void readSerialPort()
165 static byte actuatorHighByte, actuatorLowByte;
166 static byte readingSM = 0;
168 if (Serial.available())
172 actuatorHighByte = Serial.read();
173 if (actuatorHighByte >= 128) readingSM = 1;
175 else if (readingSM == 1)
177 actuatorLowByte = Serial.read();
178 if (actuatorLowByte < 128) readingSM = 2;
184 lastDataReceivedTime = millis();
185 pin = ((actuatorHighByte >> 3) & 0x0F);
186 newVal = ((actuatorHighByte & 0x07) << 7) | (actuatorLowByte & 0x7F);
188 if(arduinoPins[pin].state != newVal)
190 arduinoPins[pin].state = newVal;
196 else checkScratchDisconnection();
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
201 resetPins(); // reset pins
202 sendSensorValues(); // protocol handshaking
203 lastDataReceivedTime = millis();
206 void updateActuator(byte pinNumber)
208 if (arduinoPins[pinNumber].type==digital) digitalWrite(pinNumber, arduinoPins[pinNumber].state);
209 else if (arduinoPins[pinNumber].type==pwm) analogWrite(pinNumber, arduinoPins[pinNumber].state);
212 void sendUpdateServomotors()
214 for (byte p = 0; p < 10; p++)
215 if (arduinoPins[p].type == servomotor) servo(p, arduinoPins[p].state);
218 void servo (byte pinNumber, byte angle)
221 pulse(pinNumber, (angle * 10) + 600);
224 void pulse (byte pinNumber, unsigned int pulseWidth)
226 digitalWrite(pinNumber, HIGH);
227 delayMicroseconds(pulseWidth);
228 digitalWrite(pinNumber, LOW);
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)
233 if (millis() - lastDataReceivedTime > 1000) reset(); //reset state if actuators reception timeout = one second