Energy Efficiency with Snap Circuits - Miniature House

1// NEW IN VERSION 1.6c (by Jorge Gomez):
2// Fixed variable type in  pin structure: pi q q q q q q q q qn.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
43typedef  enum { 
44  input, servomotor, pwm, digital } 
45pinType;
46
47typedef 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
53pin  arduinoPins[14];  //Array of struct holding 0-13 pins information
54
55unsigned  long lastDataReceivedTime = millis();
56
57void setup()
58{
59  Serial.begin(38400);
60  Serial.flush();
61  configurePins();
62  resetPins();
63}
64
65void 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
79void 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
97void 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
117void 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
139void 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
146void  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
153void  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
161void 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
199void  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
206void 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
212void sendUpdateServomotors()
213{
214  for (byte p = 0; p < 10; p++)
215    if (arduinoPins[p].type == servomotor) servo(p,  arduinoPins[p].state);
216}
217
218void servo (byte pinNumber, byte angle)
219{
220  if (angle != 255)
221    pulse(pinNumber, (angle * 10) + 600);
222}
223
224void  pulse (byte pinNumber, unsigned int pulseWidth)
225{
226  digitalWrite(pinNumber,  HIGH);
227  delayMicroseconds(pulseWidth);
228  digitalWrite(pinNumber, LOW);
229}
230
231void  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
238