├── .gitattributes ├── .gitignore ├── Arduino ├── PiconZero08.ino ├── PiconZero09 │ └── PiconZero09.ino └── PiconZero10.ino ├── Python ├── 10lineTest.py ├── buttonTest.py ├── hcsr04.py ├── ioTest.py ├── motorTest.py ├── piconzero.py ├── pixelTest.py ├── servoTest.py ├── sonarTest.py ├── tempTest.py └── version.py └── readme.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /Arduino/PiconZero08.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////// 2 | // 3 | // Picon Zero Control Software 4 | // 5 | // Gareth Davies 2016 6 | // 7 | ////////////////////////////////////////////////// 8 | 9 | /* Picon Zero CONTROL 10 | - 11 | - 2 motors driven by H-Bridge: (4 outputs) 12 | - 6 general purpose outputs: Can be LOW, HIGH, PWM, Servo or WS2812B 13 | - 4 general purpose inputs: Can be Analog or Digital 14 | - 15 | - Each I2C packet comprises a Register/Command Pair of bytes 16 | 17 | Write Only Registers 18 | -------------------- 19 | Register Name Type Values 20 | 0 MotorA_Data Byte -100 (full reverse) to +100 (full forward) 21 | 1 MotorB_Data Byte -100 (full reverse) to +100 (full forward) 22 | 2 Output0_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 23 | 3 Output1_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 24 | 4 Output2_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 25 | 5 Output3_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 26 | 6 Output4_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 27 | 7 Output5_Config Byte 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 28 | 8 Output0_Data Byte Data value(s) 29 | 9 Output1_Data Byte Data value(s) 30 | 10 Output2_Data Byte Data value(s) 31 | 11 Output3_Data Byte Data value(s) 32 | 12 Output4_Data Byte Data value(s) 33 | 13 Output5_Data Byte Data value(s) 34 | 14 Input0_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup) 35 | 15 Input1_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup) 36 | 16 Input2_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup) 37 | 17 Input3_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup) 38 | 18 Set Brightness Byte 0..255. Scaled max brightness (default is 40) 39 | 19 Update Pixels Byte dummy value - forces updating of neopixels 40 | 20 Reset Byte dummy value - resets all values to initial state 41 | 42 | Read Only Registers - These are WORDs 43 | ------------------------------------- 44 | Register Name Type Values 45 | 0 Revision Word Low Byte: Firmware Build, High Byte: PCB Revision 46 | 1 Input0_Data Word 0 or 1 for Digital, 0..1023 for Analog 47 | 2 Input1_Data Word 0 or 1 for Digital, 0..1023 for Analog 48 | 3 Input2_Data Word 0 or 1 for Digital, 0..1023 for Analog 49 | 4 Input3_Data Word 0 or 1 for Digital, 0..1023 for Analog 50 | 51 | Data Values for Output Data Registers 52 | -------------------------------------- 53 | Mode Name Type Values 54 | 0 On/Off Byte 0 is OFF, 1 is ON 55 | 1 PWM Byte 0 to 100 percentage of ON time 56 | 2 Servo Byte -100 to + 100 Position in degrees 57 | 3 WS2812B 4 Bytes 0:Pixel ID, 1:Red, 2:Green, 3:Blue 58 | 59 | */ 60 | 61 | /* Rev08: Adds Pullup option for Digital and DS18B20 inputs 62 | */ 63 | 64 | #define DEBUG false 65 | #define BOARD_REV 2 // Board ID for PiconZero 66 | #define FIRMWARE_REV 8 // Firmware Revision 67 | 68 | #define MOTORA_DATA 0 69 | #define MOTORB_DATA 1 70 | #define OUTPUT0_CFG 2 71 | #define OUTPUT1_CFG 3 72 | #define OUTPUT2_CFG 4 73 | #define OUTPUT3_CFG 5 74 | #define OUTPUT4_CFG 6 75 | #define OUTPUT5_CFG 7 76 | #define OUTPUT0_DATA 8 77 | #define OUTPUT1_DATA 9 78 | #define OUTPUT2_DATA 10 79 | #define OUTPUT3_DATA 11 80 | #define OUTPUT4_DATA 12 81 | #define OUTPUT5_DATA 13 82 | #define INPUT0_CFG 14 83 | #define INPUT1_CFG 15 84 | #define INPUT2_CFG 16 85 | #define INPUT3_CFG 17 86 | #define SET_BRIGHT 18 87 | #define UPDATE_NOW 19 88 | #define RESET 20 89 | 90 | 91 | #include 92 | #include 93 | #include "FastLED.h" 94 | #include 95 | 96 | #define I2CADDR 0x22 97 | #define NUMMOTORS 10 // includes 2 for each motor as well as the general purpose output pins 98 | #define NUMOUTPUTS 6 // number of general purpose outputs 99 | #define NUMINPUTS 4 // number of Digital/Analog inputs 100 | #define NUMSERVOS 6 // possible number of servos, all configurable 101 | 102 | // Output Config Values 103 | #define CFGONOFF 0 104 | #define CFGPWM 1 105 | #define CFGSERVO 2 106 | #define CFG2812 3 107 | 108 | // Input COnfig Values 109 | #define CFGDIG 0 110 | #define CFGDIGPU 0x80 111 | #define CFGANA 1 112 | #define CFG18B20 2 113 | #define CFGDHT11 3 114 | 115 | //#define SERVO0 9 116 | //#define SERVO1 10 117 | 118 | // WS2812B definitions 119 | #define NUM_LEDS 64 120 | #define DATA_PIN 4 121 | #define BRIGHT 40 122 | #define OUTPUTWS 5 // Output ID that is allowed to use WS2812B mode 123 | 124 | CRGB leds[NUM_LEDS]; 125 | bool doShow = false; 126 | bool Done2812 = false; 127 | 128 | // Constants for Output Pins 129 | #define out0 9 130 | #define out1 10 131 | #define out2 2 132 | #define out3 3 133 | #define out4 4 134 | #define out5 5 135 | 136 | // DS18S20 Temperature chip i/o 137 | OneWire ds0(A0); // on pin A0 138 | OneWire ds1(A1); // on pin A1 139 | OneWire ds2(A2); // on pin A2 140 | OneWire ds3(A3); // on pin A3 141 | 142 | // global address data for DS18B20 input devices 143 | byte B20_addr0[8]; 144 | byte B20_addr1[8]; 145 | byte B20_addr2[8]; 146 | byte B20_addr3[8]; 147 | 148 | // PWM runs from 0 to 100 with 0 being fully OFF and 100 being fully ON 149 | // Outputs are treated as motors (if set to PWM mode) 150 | byte pwm[NUMMOTORS] = {0, 0, 0, 0, 0, 0, 0, 0}; // array of PWM values, one value for each motor/output. Value is the point in the cycle that the PWM pin is switched OFF, with it being switch ON at 0. 100 means always ON: 0 means always OFF 151 | byte motors[NUMMOTORS] = {6, 7, 8, 11, out0, out1, out2, out3, out4, out5}; // array of pins to use for each motor/ouput. Motors have 2 pins, each output has 1 pin 152 | const byte outputs[NUMOUTPUTS] = {out0, out1, out2, out3, out4, out5}; 153 | byte inputs[NUMINPUTS] = {A0, A1, A2, A3}; 154 | Servo servos[NUMSERVOS]; 155 | int inputValues[NUMINPUTS]; // store analog input values (words) 156 | byte outputConfigs[NUMOUTPUTS] = {0, 0, 0, 0, 0, 0}; // 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 157 | byte inputConfigs[NUMINPUTS] = {0, 0, 0, 0}; // 0: Digital, 1:Analog 158 | byte inputChannel = 0; // selected reading channel 159 | byte pwmcount = 0; 160 | int i; 161 | 162 | void setup() 163 | { 164 | Wire.begin(I2CADDR); // join i2c bus with defined address 165 | Wire.onRequest(requestEvent); // register event 166 | Wire.onReceive(receiveEvent); // register wire.write interrupt event 167 | if (DEBUG) 168 | { 169 | Serial.begin(115200); 170 | Serial.println("Starting..."); 171 | delay(1000); 172 | } 173 | for (i = 0; i < NUMMOTORS; i++) 174 | pinMode (motors[i], OUTPUT); 175 | FastLED.addLeds(leds, NUM_LEDS); // always have WS2812B enabled on output 5 176 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 177 | 178 | resetAll(); // initialise all I/O to safe values 179 | } 180 | 181 | // Reset routine should be called at start and end of all programs to ensure that board is set to correct configuration. Python library routines init() and cleanup() will action this 182 | void resetAll() 183 | { 184 | //disconnect all servos 185 | for (i=0; i0 && pwm[i]<100) // PWM values of 0 or 100 means no PWM, so never change this pin 212 | { 213 | //Serial.println("High:" + String(motors[i])); 214 | digitalWrite (motors[i], HIGH); 215 | } 216 | } 217 | else 218 | { 219 | for (i=0; i 99) // as pwmcount never goes over 99, then a value of 100+ will never be changed to LOW so will be on permanently 229 | { 230 | pwmcount = 0; 231 | for (int i=0; i>8; 265 | } 266 | Wire.write(outBuf, 2); 267 | } 268 | 269 | 270 | // function that executes whenever data is received from master 271 | void receiveEvent(int count) 272 | { 273 | //return; 274 | if (DEBUG) 275 | Serial.println("Data count:" + String(count)); 276 | if (count == 1) // Read request register 277 | { 278 | inputChannel = Wire.read(); 279 | if (DEBUG) 280 | Serial.println("Channel:" + String(inputChannel)); 281 | } 282 | else if (count == 2) 283 | { 284 | byte regSel = Wire.read(); 285 | byte regVal = Wire.read(); 286 | if (DEBUG) 287 | Serial.println("Register:" + String(regSel) + " Value:" + String(regVal)); 288 | switch(regSel) 289 | { 290 | case MOTORA_DATA: setMotor(0, regVal); break; 291 | case MOTORB_DATA: setMotor(1, regVal); break; 292 | case OUTPUT0_CFG: setOutCfg(0, regVal); break; 293 | case OUTPUT1_CFG: setOutCfg(1, regVal); break; 294 | case OUTPUT2_CFG: setOutCfg(2, regVal); break; 295 | case OUTPUT3_CFG: setOutCfg(3, regVal); break; 296 | case OUTPUT4_CFG: setOutCfg(4, regVal); break; 297 | case OUTPUT5_CFG: setOutCfg(5, regVal); break; 298 | case OUTPUT0_DATA: setOutData(0, regVal); break; 299 | case OUTPUT1_DATA: setOutData(1, regVal); break; 300 | case OUTPUT2_DATA: setOutData(2, regVal); break; 301 | case OUTPUT3_DATA: setOutData(3, regVal); break; 302 | case OUTPUT4_DATA: setOutData(4, regVal); break; 303 | case OUTPUT5_DATA: setOutData(5, regVal); break; 304 | case INPUT0_CFG: setInCfg(0, regVal); break; 305 | case INPUT1_CFG: setInCfg(1, regVal); break; 306 | case INPUT2_CFG: setInCfg(2, regVal); break; 307 | case INPUT3_CFG: setInCfg(3, regVal); break; 308 | case SET_BRIGHT: FastLED.setBrightness(regVal); break; 309 | case UPDATE_NOW: doShow = true; break; 310 | case RESET: resetAll(); break; 311 | } 312 | } 313 | else if (count == 5) 314 | { 315 | byte updates = Wire.read(); 316 | byte pixel = Wire.read(); 317 | byte red = Wire.read(); 318 | byte green = Wire.read(); 319 | byte blue = Wire.read(); 320 | //if (DEBUG) 321 | // Serial.println("Reg:Val::" + String(regSel) + ":" + String(pixel) + " Red:" + String(red) + " Green:" + String(green) + " Blue:" + String(blue)); 322 | if (outputConfigs[OUTPUTWS] == CFG2812) 323 | { 324 | if (pixel < NUM_LEDS) 325 | { 326 | leds[pixel].g = red; 327 | leds[pixel].r = green; 328 | leds[pixel].b = blue; 329 | if (updates != 0) 330 | doShow = true; 331 | } 332 | else if (pixel == 100) // special case meaning ALL pixels 333 | { 334 | for (int i=0; i127)?(value-256):value; 365 | int m2 = motor * 2; 366 | if (DEBUG) 367 | Serial.println("M2:" + String(m2) + " aVal:" + String(sval)); 368 | if (motor > 1) 369 | return; // Invalid motor number 370 | if (sval == 0) 371 | { 372 | digitalWrite(motors[m2], LOW); 373 | digitalWrite(motors[m2 + 1], LOW); 374 | pwm[m2] = pwm[m2 + 1] = 0; // OFF with no PWM 375 | } 376 | else if (sval >= 100) 377 | { 378 | digitalWrite(motors[m2], HIGH); 379 | digitalWrite(motors[m2 + 1], LOW); 380 | pwm[m2] = pwm[m2 + 1] = 0; // FORWARD with no PWM 381 | } 382 | else if (sval <= -100) 383 | { 384 | digitalWrite(motors[m2], LOW); 385 | digitalWrite(motors[m2 + 1], HIGH); 386 | pwm[m2] = pwm[m2 + 1] = 0; // REVERSE with no PWM 387 | } 388 | else if (sval > 0) 389 | { 390 | digitalWrite (motors[m2], HIGH); 391 | digitalWrite (motors[m2 + 1], LOW); 392 | pwm[m2] = sval; // FORWARD with PWM 393 | pwm[m2 + 1] = 0; 394 | } 395 | else // value sval must be < 0 396 | { 397 | digitalWrite (motors[m2], LOW); 398 | digitalWrite (motors[m2 + 1], HIGH); 399 | pwm[m2] = 0; 400 | pwm[m2 + 1] = -sval; // REVERSE with PWM 401 | } 402 | } 403 | 404 | void setServo(byte servo, byte value) 405 | { 406 | if(DEBUG) 407 | Serial.println("Servo: " + String(servo) + " Value:" + String(value)); 408 | servos[servo].write(value); 409 | } 410 | 411 | void setOutCfg(byte outReg, byte value) 412 | { 413 | if (outputConfigs[outReg] == value) 414 | return; // don't attach same servo twice, or even set the same value as it currently is 415 | if (outputConfigs[outReg] != CFGSERVO && value == CFGSERVO) 416 | servos[outReg].attach(outputs[outReg]); 417 | else if (outputConfigs[outReg] == CFGSERVO && value != CFGSERVO) 418 | servos[outReg].detach(); 419 | /* if (value == CFG2812 && (Done2812 || outReg != OUTPUTWS)) // Only Output 5 can be set to WS2812, and only once 420 | return; 421 | else if (value == CFG2812 && outReg == OUTPUTWS) 422 | { 423 | FastLED.addLeds(leds, NUM_LEDS); 424 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 425 | Done2812 = true; // ensure we can only have one output for WS2812 426 | }*/ 427 | outputConfigs[outReg] = value; 428 | } 429 | 430 | void setInCfg(byte inReg, byte value) 431 | { 432 | inputConfigs[inReg] = value; 433 | if (value == CFGDIGPU) 434 | pinMode(inputs[inReg], INPUT_PULLUP); 435 | else 436 | pinMode(inputs[inReg], INPUT); 437 | if (value == CFG18B20) 438 | { 439 | switch (inReg) 440 | { 441 | case 0: ds0.reset_search(); ds0.search(B20_addr0); break; 442 | case 1: ds1.reset_search(); ds1.search(B20_addr1); break; 443 | case 2: ds2.reset_search(); ds2.search(B20_addr2); break; 444 | case 3: ds3.reset_search(); ds3.search(B20_addr3); break; 445 | } 446 | } 447 | } 448 | 449 | void setOutData(byte outReg, byte value) 450 | { 451 | switch (outputConfigs[outReg]) 452 | { 453 | case CFGONOFF: 454 | pwm[outReg + 4] = 0; 455 | if(value == 0) 456 | digitalWrite(outputs[outReg], LOW); 457 | else 458 | digitalWrite(outputs[outReg], HIGH); 459 | break; 460 | case CFGPWM: 461 | if (value == 0) 462 | { 463 | pwm[outReg + 4] = 0; 464 | digitalWrite(outputs[outReg], LOW); 465 | } 466 | else if (value >= 100) 467 | { 468 | pwm[outReg + 4] = 0; 469 | digitalWrite(outputs[outReg], HIGH); 470 | } 471 | else 472 | { 473 | pwm[outReg + 4] = min(value, 99); 474 | digitalWrite(outputs[outReg], LOW); // not strictly necessary as PWM will kick in eventually 475 | } 476 | break; 477 | case CFGSERVO: 478 | servos[outReg].write(value); 479 | break; 480 | case CFG2812: 481 | // do nothing as this shouldn't arise. 5 bytes needed to address pixels. 482 | break; 483 | } 484 | } 485 | 486 | // Turns all the LEDs to OFF 487 | void allOff() 488 | { 489 | for (int i=0; i 101 | #include 102 | #include "FastLED.h" 103 | #include 104 | #include 105 | 106 | #define I2CADDR 0x22 107 | #define NUMMOTORS 10 // includes 2 for each motor as well as the general purpose output pins 108 | #define NUMOUTPUTS 6 // number of general purpose outputs 109 | #define NUMINPUTS 4 // number of Digital/Analog inputs 110 | #define NUMSERVOS 6 // possible number of servos, all configurable 111 | 112 | // Output Config Values 113 | #define CFGONOFF 0 114 | #define CFGPWM 1 115 | #define CFGSERVO 2 116 | #define CFG2812 3 117 | 118 | // Input Config Values 119 | #define CFGDIG 0 120 | #define CFGDIGPU 0x80 121 | #define CFGANA 1 122 | #define CFG18B20 2 123 | #define CFGDHT11 3 124 | #define CFGDC 4 125 | #define CFGPWIN 5 126 | 127 | // Rising and Falling indexes for the pulse width input 128 | #define INTRISING 1 129 | #define INTFALLING 0 130 | 131 | //#define SERVO0 9 132 | //#define SERVO1 10 133 | 134 | // WS2812B definitions 135 | #define NUM_LEDS 64 136 | #define DATA_PIN 4 137 | #define BRIGHT 40 138 | #define OUTPUTWS 5 // Output ID that is allowed to use WS2812B mode 139 | 140 | CRGB leds[NUM_LEDS]; 141 | bool doShow = false; 142 | bool Done2812 = false; 143 | 144 | // Constants for Output Pins 145 | #define out0 9 146 | #define out1 10 147 | #define out2 2 148 | #define out3 3 149 | #define out4 4 150 | #define out5 5 151 | 152 | // DS18S20 Temperature chip i/o 153 | OneWire ds0(A0); // on pin A0 154 | OneWire ds1(A1); // on pin A1 155 | OneWire ds2(A2); // on pin A2 156 | OneWire ds3(A3); // on pin A3 157 | 158 | // global address data for DS18B20 input devices 159 | byte B20_addr0[8]; 160 | byte B20_addr1[8]; 161 | byte B20_addr2[8]; 162 | byte B20_addr3[8]; 163 | 164 | // PWM runs from 0 to 100 with 0 being fully OFF and 100 being fully ON 165 | // Outputs are treated as motors (if set to PWM mode) 166 | byte pwm[NUMMOTORS] = {0, 0, 0, 0, 0, 0, 0, 0}; // array of PWM values, one value for each motor/output. Value is the point in the cycle that the PWM pin is switched OFF, with it being switch ON at 0. 100 means always ON: 0 means always OFF 167 | byte motors[NUMMOTORS] = {6, 7, 8, 11, out0, out1, out2, out3, out4, out5}; // array of pins to use for each motor/ouput. Motors have 2 pins, each output has 1 pin 168 | const byte outputs[NUMOUTPUTS] = {out0, out1, out2, out3, out4, out5}; 169 | byte inputs[NUMINPUTS] = {A0, A1, A2, A3}; 170 | Servo servos[NUMSERVOS]; 171 | int inputValues[NUMINPUTS]; // store analog input values (words) 172 | volatile unsigned long interrupt[NUMINPUTS][2]; // Store rising and falling edges for pulse width inputs. 173 | int pwmPeriod[NUMINPUTS] = {2000, 2000, 2000, 2000}; // Store expected PWM period 174 | byte outputConfigs[NUMOUTPUTS] = {0, 0, 0, 0, 0, 0}; // 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 175 | byte inputConfigs[NUMINPUTS] = {0, 0, 0, 0}; // 0: Digital, 1:Analog 176 | byte inputChannel = 0; // selected reading channel 177 | byte pwmcount = 0; 178 | 179 | void setup() 180 | { 181 | Wire.begin(I2CADDR); // join i2c bus with defined address 182 | Wire.onRequest(requestEvent); // register event 183 | Wire.onReceive(receiveEvent); // register wire.write interrupt event 184 | if (DEBUG) 185 | { 186 | Serial.begin(115200); 187 | Serial.println("Starting..."); 188 | delay(1000); 189 | } 190 | for (int i = 0; i < NUMMOTORS; i++) 191 | pinMode (motors[i], OUTPUT); 192 | FastLED.addLeds(leds, NUM_LEDS); // always have WS2812B enabled on output 5 193 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 194 | 195 | resetAll(); // initialise all I/O to safe values 196 | } 197 | 198 | // Reset routine should be called at start and end of all programs to ensure that board is set to correct configuration. Python library routines init() and cleanup() will action this 199 | void resetAll() 200 | { 201 | //disconnect all servos 202 | for (int i=0; i0 && pwm[i]<100) // PWM values of 0 or 100 means no PWM, so never change this pin 231 | { 232 | //Serial.println("High:" + String(motors[i])); 233 | digitalWrite (motors[i], HIGH); 234 | } 235 | } 236 | else 237 | { 238 | for (int i=0; i 99) // as pwmcount never goes over 99, then a value of 100+ will never be changed to LOW so will be on permanently 248 | { 249 | pwmcount = 0; 250 | for (int i=0; i>8; 286 | } 287 | Wire.write(outBuf, 2); 288 | } 289 | 290 | 291 | // function that executes whenever data is received from master 292 | void receiveEvent(int count) 293 | { 294 | //return; 295 | if (DEBUG) 296 | Serial.println("Data count:" + String(count)); 297 | if (count == 1) // Read request register 298 | { 299 | inputChannel = Wire.read(); 300 | if (DEBUG) 301 | Serial.println("Channel:" + String(inputChannel)); 302 | } 303 | else if (count == 2) 304 | { 305 | byte regSel = Wire.read(); 306 | byte regVal = Wire.read(); 307 | if (DEBUG) 308 | Serial.println("Register:" + String(regSel) + " Value:" + String(regVal)); 309 | switch(regSel) 310 | { 311 | case MOTORA_DATA: setMotor(0, regVal); break; 312 | case MOTORB_DATA: setMotor(1, regVal); break; 313 | case OUTPUT0_CFG: setOutCfg(0, regVal); break; 314 | case OUTPUT1_CFG: setOutCfg(1, regVal); break; 315 | case OUTPUT2_CFG: setOutCfg(2, regVal); break; 316 | case OUTPUT3_CFG: setOutCfg(3, regVal); break; 317 | case OUTPUT4_CFG: setOutCfg(4, regVal); break; 318 | case OUTPUT5_CFG: setOutCfg(5, regVal); break; 319 | case OUTPUT0_DATA: setOutData(0, regVal); break; 320 | case OUTPUT1_DATA: setOutData(1, regVal); break; 321 | case OUTPUT2_DATA: setOutData(2, regVal); break; 322 | case OUTPUT3_DATA: setOutData(3, regVal); break; 323 | case OUTPUT4_DATA: setOutData(4, regVal); break; 324 | case OUTPUT5_DATA: setOutData(5, regVal); break; 325 | case INPUT0_CFG: setInCfg(0, regVal); break; 326 | case INPUT1_CFG: setInCfg(1, regVal); break; 327 | case INPUT2_CFG: setInCfg(2, regVal); break; 328 | case INPUT3_CFG: setInCfg(3, regVal); break; 329 | case SET_BRIGHT: FastLED.setBrightness(regVal); break; 330 | case UPDATE_NOW: doShow = true; break; 331 | case RESET: resetAll(); break; 332 | } 333 | } 334 | else if (count == 3) //Read in period, the value is read in as a word so we get to combine two bytes 335 | { 336 | byte regSel = Wire.read(); 337 | int regVal = Wire.read() | Wire.read() << 8; 338 | if (DEBUG) 339 | { 340 | Serial.println("Register:" + String(regSel) + " Value:" + String(regVal)); 341 | } 342 | 343 | switch(regSel) 344 | { 345 | case INPUT0_PERIOD: pwmPeriod[0] = regVal; break; 346 | case INPUT1_PERIOD: pwmPeriod[1] = regVal; break; 347 | case INPUT2_PERIOD: pwmPeriod[2] = regVal; break; 348 | case INPUT3_PERIOD: pwmPeriod[3] = regVal; break; 349 | } 350 | } 351 | else if (count == 5) 352 | { 353 | byte updates = Wire.read(); 354 | byte pixel = Wire.read(); 355 | byte red = Wire.read(); 356 | byte green = Wire.read(); 357 | byte blue = Wire.read(); 358 | //if (DEBUG) 359 | // Serial.println("Reg:Val::" + String(regSel) + ":" + String(pixel) + " Red:" + String(red) + " Green:" + String(green) + " Blue:" + String(blue)); 360 | if (outputConfigs[OUTPUTWS] == CFG2812) 361 | { 362 | if (pixel < NUM_LEDS) 363 | { 364 | leds[pixel].g = red; 365 | leds[pixel].r = green; 366 | leds[pixel].b = blue; 367 | if (updates != 0) 368 | doShow = true; 369 | } 370 | else if (pixel == 100) // special case meaning ALL pixels 371 | { 372 | for (int i=0; i127)?(value-256):value; 403 | int m2 = motor * 2; 404 | if (DEBUG) 405 | Serial.println("M2:" + String(m2) + " aVal:" + String(sval)); 406 | if (motor > 1) 407 | return; // Invalid motor number 408 | if (sval == 0) 409 | { 410 | digitalWrite(motors[m2], LOW); 411 | digitalWrite(motors[m2 + 1], LOW); 412 | pwm[m2] = pwm[m2 + 1] = 0; // OFF with no PWM 413 | } 414 | else if (sval >= 100) 415 | { 416 | digitalWrite(motors[m2], HIGH); 417 | digitalWrite(motors[m2 + 1], LOW); 418 | pwm[m2] = pwm[m2 + 1] = 0; // FORWARD with no PWM 419 | } 420 | else if (sval <= -100) 421 | { 422 | digitalWrite(motors[m2], LOW); 423 | digitalWrite(motors[m2 + 1], HIGH); 424 | pwm[m2] = pwm[m2 + 1] = 0; // REVERSE with no PWM 425 | } 426 | else if (sval > 0) 427 | { 428 | digitalWrite (motors[m2], HIGH); 429 | digitalWrite (motors[m2 + 1], LOW); 430 | pwm[m2] = sval; // FORWARD with PWM 431 | pwm[m2 + 1] = 0; 432 | } 433 | else // value sval must be < 0 434 | { 435 | digitalWrite (motors[m2], LOW); 436 | digitalWrite (motors[m2 + 1], HIGH); 437 | pwm[m2] = 0; 438 | pwm[m2 + 1] = -sval; // REVERSE with PWM 439 | } 440 | } 441 | 442 | void setServo(byte servo, byte value) 443 | { 444 | if(DEBUG) 445 | Serial.println("Servo: " + String(servo) + " Value:" + String(value)); 446 | servos[servo].write(value); 447 | } 448 | 449 | void setOutCfg(byte outReg, byte value) 450 | { 451 | if (outputConfigs[outReg] == value) 452 | return; // don't attach same servo twice, or even set the same value as it currently is 453 | if (outputConfigs[outReg] != CFGSERVO && value == CFGSERVO) 454 | servos[outReg].attach(outputs[outReg]); 455 | else if (outputConfigs[outReg] == CFGSERVO && value != CFGSERVO) 456 | servos[outReg].detach(); 457 | /* if (value == CFG2812 && (Done2812 || outReg != OUTPUTWS)) // Only Output 5 can be set to WS2812, and only once 458 | return; 459 | else if (value == CFG2812 && outReg == OUTPUTWS) 460 | { 461 | FastLED.addLeds(leds, NUM_LEDS); 462 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 463 | Done2812 = true; // ensure we can only have one output for WS2812 464 | }*/ 465 | outputConfigs[outReg] = value; 466 | } 467 | 468 | void setInCfg(byte inReg, byte value) 469 | { 470 | if (DEBUG) 471 | { 472 | Serial.println("inReg:" + String(inReg) + " value:" + String(value)); 473 | } 474 | inputConfigs[inReg] = value; 475 | if (value == CFGDIGPU) 476 | { 477 | pinMode(inputs[inReg], INPUT_PULLUP); 478 | } 479 | else 480 | { 481 | pinMode(inputs[inReg], INPUT); 482 | } 483 | if (value == CFG18B20) 484 | { 485 | switch (inReg) 486 | { 487 | case 0: ds0.reset_search(); ds0.search(B20_addr0); break; 488 | case 1: ds1.reset_search(); ds1.search(B20_addr1); break; 489 | case 2: ds2.reset_search(); ds2.search(B20_addr2); break; 490 | case 3: ds3.reset_search(); ds3.search(B20_addr3); break; 491 | } 492 | } 493 | if (value == CFGDC || value == CFGPWIN) // Steup interupts for the apropriate pins. 494 | { 495 | digitalWrite(inputs[inReg], HIGH); 496 | enableInterrupt(inputs[inReg], storeEdgeTime, CHANGE); 497 | inputValues[inReg] = 0; 498 | } 499 | else // disable Interrupt on any non iterrupt pins. 500 | { 501 | disableInterrupt(inputs[inReg]); 502 | } 503 | } 504 | 505 | void setOutData(byte outReg, byte value) 506 | { 507 | switch (outputConfigs[outReg]) 508 | { 509 | case CFGONOFF: 510 | pwm[outReg + 4] = 0; 511 | if(value == 0) 512 | digitalWrite(outputs[outReg], LOW); 513 | else 514 | digitalWrite(outputs[outReg], HIGH); 515 | break; 516 | case CFGPWM: 517 | if (value == 0) 518 | { 519 | pwm[outReg + 4] = 0; 520 | digitalWrite(outputs[outReg], LOW); 521 | } 522 | else if (value >= 100) 523 | { 524 | pwm[outReg + 4] = 0; 525 | digitalWrite(outputs[outReg], HIGH); 526 | } 527 | else 528 | { 529 | pwm[outReg + 4] = min(value, 99); 530 | digitalWrite(outputs[outReg], LOW); // not strictly necessary as PWM will kick in eventually 531 | } 532 | break; 533 | case CFGSERVO: 534 | servos[outReg].write(value); 535 | break; 536 | case CFG2812: 537 | // do nothing as this shouldn't arise. 5 bytes needed to address pixels. 538 | break; 539 | } 540 | } 541 | 542 | // Turns all the LEDs to OFF 543 | void allOff() 544 | { 545 | for (int i=0; i 0 && pwmRising > 0) // If we have rising and falling edges calcuate the pulse width 623 | { 624 | interrupt[index][INTRISING] = 0; // Signal calculation complete 625 | int pulseWidth = pwmFalling-pwmRising; 626 | if (inputConfig == CFGPWIN) 627 | { 628 | return pulseWidth; 629 | } 630 | else 631 | { 632 | int dutyCycle = (float)((float)(pulseWidth)/period) * 100; 633 | if (dutyCycle > 100) 634 | { 635 | dutyCycle = 100; 636 | } 637 | return dutyCycle; 638 | } 639 | } 640 | else if (periodExceeded(pwmFalling, period)) 641 | { 642 | // Signal low longer than set period. 0% duty cycle. 643 | interrupt[index][INTFALLING] = 0; 644 | return 0; 645 | } 646 | else if (periodExceeded(pwmRising, period)) 647 | { 648 | // Signal high longer than set period. 100% duty cycle or 0. 649 | interrupt[index][INTRISING] = 0; 650 | return inputConfig == CFGPWIN ? 0 : 100; 651 | } 652 | return inputValues[index]; 653 | } 654 | 655 | bool periodExceeded(unsigned long edge, unsigned long period) // Check if an edge has exceeded the the set period 656 | { 657 | return (edge > 0 && micros() - edge >= period); 658 | } 659 | 660 | // On voltage change, store data required to calculate the pwm. 661 | void storeEdgeTime() 662 | { 663 | unsigned long now = micros(); 664 | // Input pins are between 14 and 18 665 | int index = arduinoInterruptedPin - 14; 666 | // Always set falling edge to zero. If this change is the falling edge the correct value will be set next. If rising and falling are set the PWM is ready to be calculated. 667 | interrupt[index][INTFALLING] = 0; 668 | interrupt[index][arduinoPinState > 0] = now; 669 | } 670 | 671 | -------------------------------------------------------------------------------- /Arduino/PiconZero10.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////// 2 | // 3 | // Picon Zero Control Software 4 | // 5 | // Gareth Davies 2016, 2017, 2018 6 | // 7 | ////////////////////////////////////////////////// 8 | 9 | /* Picon Zero CONTROL 10 | - 11 | - 2 motors driven by H-Bridge: (4 outputs) 12 | - 6 general purpose outputs: Can be LOW, HIGH, PWM, Servo or WS2812B 13 | - 4 general purpose inputs: Can be Analog or Digital 14 | - 15 | - Each I2C packet comprises a Register/Command Pair of bytes 16 | 17 | Write Only Registers 18 | -------------------- 19 | Register Name Type Values 20 | 0 MotorA_Data Byte -100 (full reverse) to +100 (full forward) 21 | 1 MotorB_Data Byte -100 (full reverse) to +100 (full forward) 22 | 2 Output0_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 23 | 3 Output1_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 24 | 4 Output2_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 25 | 5 Output3_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 26 | 6 Output4_Config Byte 0: On/Off, 1: PWM, 2: Servo, (3: WS2812B) 27 | 7 Output5_Config Byte 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 28 | 8 Output0_Data Byte Data value(s) 29 | 9 Output1_Data Byte Data value(s) 30 | 10 Output2_Data Byte Data value(s) 31 | 11 Output3_Data Byte Data value(s) 32 | 12 Output4_Data Byte Data value(s) 33 | 13 Output5_Data Byte Data value(s) 34 | 14 Input0_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup), 4:Duty Cycle 5: PW 35 | 15 Input1_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup), 4:Duty Cycle 5: PW 36 | 16 Input2_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup), 4:Duty Cycle 5: PW 37 | 17 Input3_Config Byte 0: Digital, 1:Analog, 2:DS18B20, 3:DHT11 (NB. 0x80 is Digital input with pullup), 4:Duty Cycle 5: PW 38 | 18 Set Brightness Byte 0..255. Scaled max brightness (default is 40) 39 | 19 Update Pixels Byte dummy value - forces updating of neopixels 40 | 20 Reset Byte dummy value - resets all values to initial state 41 | 42 | Read Only Registers - These are WORDs 43 | ------------------------------------- 44 | Register Name Type Values 45 | 0 Revision Word Low Byte: Firmware Build, High Byte: PCB Revision 46 | 1 Input0_Data Word 0 or 1 for Digital, 0..1023 for Analog 47 | 2 Input1_Data Word 0 or 1 for Digital, 0..1023 for Analog 48 | 3 Input2_Data Word 0 or 1 for Digital, 0..1023 for Analog 49 | 4 Input3_Data Word 0 or 1 for Digital, 0..1023 for Analog 50 | 51 | Data Values for Output Data Registers 52 | -------------------------------------- 53 | Mode Name Type Values 54 | 0 On/Off Byte 0 is OFF, 1 is ON 55 | 1 PWM Byte 0 to 100 percentage of ON time 56 | 2 Servo Byte -100 to + 100 Position in degrees 57 | 3 WS2812B 4 Bytes 0:Pixel ID, 1:Red, 2:Green, 3:Blue 58 | 59 | */ 60 | 61 | /* Rev08: Adds Pullup option for Digital and DS18B20 inputs 62 | */ 63 | 64 | #define DEBUG false 65 | #define BOARD_REV 2 // Board ID for PiconZero 66 | #define FIRMWARE_REV 10 // Firmware Revision 67 | 68 | #define MOTORA_DATA 0 69 | #define MOTORB_DATA 1 70 | #define OUTPUT0_CFG 2 71 | #define OUTPUT1_CFG 3 72 | #define OUTPUT2_CFG 4 73 | #define OUTPUT3_CFG 5 74 | #define OUTPUT4_CFG 6 75 | #define OUTPUT5_CFG 7 76 | #define OUTPUT0_DATA 8 77 | #define OUTPUT1_DATA 9 78 | #define OUTPUT2_DATA 10 79 | #define OUTPUT3_DATA 11 80 | #define OUTPUT4_DATA 12 81 | #define OUTPUT5_DATA 13 82 | #define INPUT0_CFG 14 83 | #define INPUT1_CFG 15 84 | #define INPUT2_CFG 16 85 | #define INPUT3_CFG 17 86 | #define SET_BRIGHT 18 87 | #define UPDATE_NOW 19 88 | #define RESET 20 89 | #define INPUT0_PERIOD 21 90 | #define INPUT1_PERIOD 22 91 | #define INPUT2_PERIOD 23 92 | #define INPUT3_PERIOD 24 93 | 94 | // Enable Interrupt setup 95 | #define EI_ARDUINO_INTERRUPTED_PIN 96 | #define EI_NOTEXTERNAL 97 | #define EI_NOTPORTB 98 | #define EI_NOTPORTD 99 | 100 | #include 101 | #include 102 | #include "FastLED.h" 103 | #include 104 | //#include 105 | 106 | #define I2CADDR 0x22 107 | #define NUMMOTORS 10 // includes 2 for each motor as well as the general purpose output pins 108 | #define NUMOUTPUTS 6 // number of general purpose outputs 109 | #define NUMINPUTS 4 // number of Digital/Analog inputs 110 | #define NUMSERVOS 6 // possible number of servos, all configurable 111 | 112 | // Output Config Values 113 | #define CFGONOFF 0 114 | #define CFGPWM 1 115 | #define CFGSERVO 2 116 | #define CFG2812 3 117 | 118 | // Input Config Values 119 | #define CFGDIG 0 120 | #define CFGDIGPU 0x80 121 | #define CFGANA 1 122 | #define CFG18B20 2 123 | #define CFGDHT11 3 124 | #define CFGDC 4 125 | #define CFGPWIN 5 126 | 127 | // Rising and Falling indexes for the pulse width input 128 | #define INTRISING 1 129 | #define INTFALLING 0 130 | 131 | //#define SERVO0 9 132 | //#define SERVO1 10 133 | 134 | // WS2812B definitions 135 | #define NUM_LEDS 64 136 | #define DATA_PIN 4 137 | #define BRIGHT 40 138 | #define OUTPUTWS 5 // Output ID that is allowed to use WS2812B mode 139 | 140 | CRGB leds[NUM_LEDS]; 141 | bool doShow = false; 142 | bool Done2812 = false; 143 | 144 | // Constants for Output Pins 145 | #define out0 9 146 | #define out1 10 147 | #define out2 2 148 | #define out3 3 149 | #define out4 4 150 | #define out5 5 151 | 152 | // DS18S20 Temperature chip i/o 153 | OneWire ds0(A0); // on pin A0 154 | OneWire ds1(A1); // on pin A1 155 | OneWire ds2(A2); // on pin A2 156 | OneWire ds3(A3); // on pin A3 157 | 158 | // global address data for DS18B20 input devices 159 | byte B20_addr0[8]; 160 | byte B20_addr1[8]; 161 | byte B20_addr2[8]; 162 | byte B20_addr3[8]; 163 | 164 | // PWM runs from 0 to 100 with 0 being fully OFF and 100 being fully ON 165 | // Outputs are treated as motors (if set to PWM mode) 166 | byte pwm[NUMMOTORS] = {0, 0, 0, 0, 0, 0, 0, 0}; // array of PWM values, one value for each motor/output. Value is the point in the cycle that the PWM pin is switched OFF, with it being switch ON at 0. 100 means always ON: 0 means always OFF 167 | byte motors[NUMMOTORS] = {6, 7, 8, 11, out0, out1, out2, out3, out4, out5}; // array of pins to use for each motor/ouput. Motors have 2 pins, each output has 1 pin 168 | const byte outputs[NUMOUTPUTS] = {out0, out1, out2, out3, out4, out5}; 169 | byte inputs[NUMINPUTS] = {A0, A1, A2, A3}; 170 | Servo servos[NUMSERVOS]; 171 | int inputValues[NUMINPUTS]; // store analog input values (words) 172 | volatile unsigned long interrupt[NUMINPUTS][2]; // Store rising and falling edges for pulse width inputs. 173 | int pwmPeriod[NUMINPUTS] = {2000, 2000, 2000, 2000}; // Store expected PWM period 174 | byte outputConfigs[NUMOUTPUTS] = {0, 0, 0, 0, 0, 0}; // 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 175 | byte inputConfigs[NUMINPUTS] = {0, 0, 0, 0}; // 0: Digital, 1:Analog 176 | byte inputChannel = 0; // selected reading channel 177 | byte pwmcount = 0; 178 | 179 | void setup() 180 | { 181 | Wire.begin(I2CADDR); // join i2c bus with defined address 182 | Wire.onRequest(requestEvent); // register event 183 | Wire.onReceive(receiveEvent); // register wire.write interrupt event 184 | if (DEBUG) 185 | { 186 | Serial.begin(115200); 187 | Serial.println("Starting..."); 188 | delay(1000); 189 | } 190 | for (int i = 0; i < NUMMOTORS; i++) 191 | pinMode (motors[i], OUTPUT); 192 | FastLED.addLeds(leds, NUM_LEDS); // always have WS2812B enabled on output 5 193 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 194 | 195 | resetAll(); // initialise all I/O to safe values 196 | } 197 | 198 | // Reset routine should be called at start and end of all programs to ensure that board is set to correct configuration. Python library routines init() and cleanup() will action this 199 | void resetAll() 200 | { 201 | //disconnect all servos 202 | for (int i=0; i0 && pwm[i]<100) 237 | { 238 | if((pwm[i]>59 && pwmcount==0) || (pwm[i]>39 && pwm[i]<60 && pmc2==0) || (pwm[i]>29 && pwm[i]<40 && pmc4==0) || (pwm[i]>19 && pwm[i]<30 && pmc8==0) || (pwm[i]<20 && pmc16==0)) 239 | digitalWrite (motors[i], HIGH); 240 | if((pwm[i]>59 && pwm[i]==pwmcount) || (pwm[i]>39 && pwm[i]<60 && pwm[i]==pmc2) || (pwm[i]>29 && pwm[i]<40 && pwm[i]==pmc4) || (pwm[i]>19 && pwm[i]<30 && pwm[i]==pmc8) || (pwm[i]<20 && pwm[i]==pmc16)) 241 | digitalWrite (motors[i], LOW); 242 | } 243 | } 244 | if (pwmcount == 0) 245 | { 246 | for (int i=4; i0 && pwm[i]<100) // PWM values of 0 or 100 means no PWM, so never change this pin 248 | { 249 | //Serial.println("High:" + String(motors[i])); 250 | digitalWrite (motors[i], HIGH); 251 | } 252 | } 253 | else 254 | { 255 | for (int i=4; i 99) 269 | pmc2 = 0; 270 | if ((pmc % 4) == 0) 271 | { 272 | if (++pmc4 > 99) 273 | pmc4 = 0; 274 | if ((pmc % 8) == 0) 275 | { 276 | if(++pmc8 > 99) 277 | pmc8 = 0; 278 | if ((pmc % 16) == 0) 279 | { 280 | if(++pmc16 > 99) 281 | pmc16 = 0; 282 | } 283 | } 284 | } 285 | } 286 | if (++pwmcount > 99) // as pwmcount never goes over 99, then a value of 100+ will never be changed to LOW so will be on permanently 287 | { 288 | pwmcount = 0; 289 | for (int i=0; i>8; 325 | } 326 | Wire.write(outBuf, 2); 327 | } 328 | 329 | 330 | // function that executes whenever data is received from master 331 | void receiveEvent(int count) 332 | { 333 | //return; 334 | if (DEBUG) 335 | Serial.println("Data count:" + String(count)); 336 | if (count == 1) // Read request register 337 | { 338 | inputChannel = Wire.read(); 339 | if (DEBUG) 340 | Serial.println("Channel:" + String(inputChannel)); 341 | } 342 | else if (count == 2) 343 | { 344 | byte regSel = Wire.read(); 345 | byte regVal = Wire.read(); 346 | if (DEBUG) 347 | Serial.println("Register:" + String(regSel) + " Value:" + String(regVal)); 348 | switch(regSel) 349 | { 350 | case MOTORA_DATA: setMotor(0, regVal); break; 351 | case MOTORB_DATA: setMotor(1, regVal); break; 352 | case OUTPUT0_CFG: setOutCfg(0, regVal); break; 353 | case OUTPUT1_CFG: setOutCfg(1, regVal); break; 354 | case OUTPUT2_CFG: setOutCfg(2, regVal); break; 355 | case OUTPUT3_CFG: setOutCfg(3, regVal); break; 356 | case OUTPUT4_CFG: setOutCfg(4, regVal); break; 357 | case OUTPUT5_CFG: setOutCfg(5, regVal); break; 358 | case OUTPUT0_DATA: setOutData(0, regVal); break; 359 | case OUTPUT1_DATA: setOutData(1, regVal); break; 360 | case OUTPUT2_DATA: setOutData(2, regVal); break; 361 | case OUTPUT3_DATA: setOutData(3, regVal); break; 362 | case OUTPUT4_DATA: setOutData(4, regVal); break; 363 | case OUTPUT5_DATA: setOutData(5, regVal); break; 364 | case INPUT0_CFG: setInCfg(0, regVal); break; 365 | case INPUT1_CFG: setInCfg(1, regVal); break; 366 | case INPUT2_CFG: setInCfg(2, regVal); break; 367 | case INPUT3_CFG: setInCfg(3, regVal); break; 368 | case SET_BRIGHT: FastLED.setBrightness(regVal); break; 369 | case UPDATE_NOW: doShow = true; break; 370 | case RESET: resetAll(); break; 371 | } 372 | } 373 | else if (count == 3) //Read in period, the value is read in as a word so we get to combine two bytes 374 | { 375 | byte regSel = Wire.read(); 376 | int regVal = Wire.read() | Wire.read() << 8; 377 | if (DEBUG) 378 | { 379 | Serial.println("Register:" + String(regSel) + " Value:" + String(regVal)); 380 | } 381 | 382 | switch(regSel) 383 | { 384 | case INPUT0_PERIOD: pwmPeriod[0] = regVal; break; 385 | case INPUT1_PERIOD: pwmPeriod[1] = regVal; break; 386 | case INPUT2_PERIOD: pwmPeriod[2] = regVal; break; 387 | case INPUT3_PERIOD: pwmPeriod[3] = regVal; break; 388 | } 389 | } 390 | else if (count == 5) 391 | { 392 | byte updates = Wire.read(); 393 | byte pixel = Wire.read(); 394 | byte red = Wire.read(); 395 | byte green = Wire.read(); 396 | byte blue = Wire.read(); 397 | //if (DEBUG) 398 | // Serial.println("Reg:Val::" + String(regSel) + ":" + String(pixel) + " Red:" + String(red) + " Green:" + String(green) + " Blue:" + String(blue)); 399 | if (outputConfigs[OUTPUTWS] == CFG2812) 400 | { 401 | if (pixel < NUM_LEDS) 402 | { 403 | leds[pixel].g = red; 404 | leds[pixel].r = green; 405 | leds[pixel].b = blue; 406 | if (updates != 0) 407 | doShow = true; 408 | } 409 | else if (pixel == 100) // special case meaning ALL pixels 410 | { 411 | for (int i=0; i127)?(value-256):value; 442 | int m2 = motor * 2; 443 | if (DEBUG) 444 | Serial.println("M2:" + String(m2) + " aVal:" + String(sval)); 445 | if (motor > 1) 446 | return; // Invalid motor number 447 | if (sval == 0) 448 | { 449 | digitalWrite(motors[m2], LOW); 450 | digitalWrite(motors[m2 + 1], LOW); 451 | pwm[m2] = pwm[m2 + 1] = 0; // OFF with no PWM 452 | } 453 | else if (sval >= 100) 454 | { 455 | digitalWrite(motors[m2], HIGH); 456 | digitalWrite(motors[m2 + 1], LOW); 457 | pwm[m2] = pwm[m2 + 1] = 0; // FORWARD with no PWM 458 | } 459 | else if (sval <= -100) 460 | { 461 | digitalWrite(motors[m2], LOW); 462 | digitalWrite(motors[m2 + 1], HIGH); 463 | pwm[m2] = pwm[m2 + 1] = 0; // REVERSE with no PWM 464 | } 465 | else if (sval > 0) 466 | { 467 | //digitalWrite (motors[m2], HIGH); // Don't set the PWM side or it jitters when new commands are sent 468 | digitalWrite (motors[m2 + 1], LOW); 469 | pwm[m2] = sval; // FORWARD with PWM 470 | pwm[m2 + 1] = 0; 471 | } 472 | else // value sval must be < 0 473 | { 474 | digitalWrite (motors[m2], LOW); 475 | //digitalWrite (motors[m2 + 1], HIGH); // Don't set the PWM side or it jitters when new commands are sent 476 | pwm[m2] = 0; 477 | pwm[m2 + 1] = -sval; // REVERSE with PWM 478 | } 479 | } 480 | 481 | void setServo(byte servo, byte value) 482 | { 483 | if(DEBUG) 484 | Serial.println("Servo: " + String(servo) + " Value:" + String(value)); 485 | servos[servo].write(value); 486 | } 487 | 488 | void setOutCfg(byte outReg, byte value) 489 | { 490 | if (outputConfigs[outReg] == value) 491 | return; // don't attach same servo twice, or even set the same value as it currently is 492 | if (outputConfigs[outReg] != CFGSERVO && value == CFGSERVO) 493 | servos[outReg].attach(outputs[outReg]); 494 | else if (outputConfigs[outReg] == CFGSERVO && value != CFGSERVO) 495 | servos[outReg].detach(); 496 | /* if (value == CFG2812 && (Done2812 || outReg != OUTPUTWS)) // Only Output 5 can be set to WS2812, and only once 497 | return; 498 | else if (value == CFG2812 && outReg == OUTPUTWS) 499 | { 500 | FastLED.addLeds(leds, NUM_LEDS); 501 | FastLED.setBrightness(BRIGHT); // sets the maximum brightness level. All values are scaled to fit in this range 502 | Done2812 = true; // ensure we can only have one output for WS2812 503 | }*/ 504 | outputConfigs[outReg] = value; 505 | } 506 | 507 | void setInCfg(byte inReg, byte value) 508 | { 509 | if (DEBUG) 510 | { 511 | Serial.println("inReg:" + String(inReg) + " value:" + String(value)); 512 | } 513 | inputConfigs[inReg] = value; 514 | if (value == CFGDIGPU) 515 | { 516 | pinMode(inputs[inReg], INPUT_PULLUP); 517 | } 518 | else 519 | { 520 | pinMode(inputs[inReg], INPUT); 521 | } 522 | if (value == CFG18B20) 523 | { 524 | switch (inReg) 525 | { 526 | case 0: ds0.reset_search(); ds0.search(B20_addr0); break; 527 | case 1: ds1.reset_search(); ds1.search(B20_addr1); break; 528 | case 2: ds2.reset_search(); ds2.search(B20_addr2); break; 529 | case 3: ds3.reset_search(); ds3.search(B20_addr3); break; 530 | } 531 | } 532 | if (value == CFGDC || value == CFGPWIN) // Steup interupts for the apropriate pins. 533 | { 534 | digitalWrite(inputs[inReg], HIGH); 535 | // enableInterrupt(inputs[inReg], storeEdgeTime, CHANGE); 536 | inputValues[inReg] = 0; 537 | } 538 | else // disable Interrupt on any non iterrupt pins. 539 | { 540 | // disableInterrupt(inputs[inReg]); 541 | } 542 | } 543 | 544 | void setOutData(byte outReg, byte value) 545 | { 546 | switch (outputConfigs[outReg]) 547 | { 548 | case CFGONOFF: 549 | pwm[outReg + 4] = 0; 550 | if(value == 0) 551 | digitalWrite(outputs[outReg], LOW); 552 | else 553 | digitalWrite(outputs[outReg], HIGH); 554 | break; 555 | case CFGPWM: 556 | if (value == 0) 557 | { 558 | pwm[outReg + 4] = 0; 559 | digitalWrite(outputs[outReg], LOW); 560 | } 561 | else if (value >= 100) 562 | { 563 | pwm[outReg + 4] = 0; 564 | digitalWrite(outputs[outReg], HIGH); 565 | } 566 | else 567 | { 568 | pwm[outReg + 4] = min(value, 99); 569 | digitalWrite(outputs[outReg], LOW); // not strictly necessary as PWM will kick in eventually 570 | } 571 | break; 572 | case CFGSERVO: 573 | servos[outReg].write(value); 574 | break; 575 | case CFG2812: 576 | // do nothing as this shouldn't arise. 5 bytes needed to address pixels. 577 | break; 578 | } 579 | } 580 | 581 | // Turns all the LEDs to OFF 582 | void allOff() 583 | { 584 | for (int i=0; i 0 && pwmRising > 0) // If we have rising and falling edges calcuate the pulse width 662 | { 663 | interrupt[index][INTRISING] = 0; // Signal calculation complete 664 | int pulseWidth = pwmFalling-pwmRising; 665 | if (inputConfig == CFGPWIN) 666 | { 667 | return pulseWidth; 668 | } 669 | else 670 | { 671 | int dutyCycle = (float)((float)(pulseWidth)/period) * 100; 672 | if (dutyCycle > 100) 673 | { 674 | dutyCycle = 100; 675 | } 676 | return dutyCycle; 677 | } 678 | } 679 | else if (periodExceeded(pwmFalling, period)) 680 | { 681 | // Signal low longer than set period. 0% duty cycle. 682 | interrupt[index][INTFALLING] = 0; 683 | return 0; 684 | } 685 | else if (periodExceeded(pwmRising, period)) 686 | { 687 | // Signal high longer than set period. 100% duty cycle or 0. 688 | interrupt[index][INTRISING] = 0; 689 | return inputConfig == CFGPWIN ? 0 : 100; 690 | } 691 | return inputValues[index]; 692 | } 693 | 694 | bool periodExceeded(unsigned long edge, unsigned long period) // Check if an edge has exceeded the the set period 695 | { 696 | return (edge > 0 && micros() - edge >= period); 697 | } 698 | 699 | // On voltage change, store data required to calculate the pwm. 700 | void storeEdgeTime() 701 | { 702 | unsigned long now = micros(); 703 | // Input pins are between 14 and 18 704 | // int index = arduinoInterruptedPin - 14; 705 | // Always set falling edge to zero. If this change is the falling edge the correct value will be set next. If rising and falling are set the PWM is ready to be calculated. 706 | // interrupt[index][INTFALLING] = 0; 707 | // interrupt[index][arduinoPinState > 0] = now; 708 | } 709 | 710 | -------------------------------------------------------------------------------- /Python/10lineTest.py: -------------------------------------------------------------------------------- 1 | 2 | import piconzero as pz, time 3 | 4 | pz.setInputConfig(0, 1) # set input 0 to Analog 5 | pz.setOutputConfig(0, 1) # set output 0 to PWM 6 | pz.setOutputConfig(2, 2) # set output 2 to Servo 7 | pz.setOutputConfig(5, 3) # set output 5 to WS2812 8 | 9 | while True: 10 | ana0 = pz.readInput(0) 11 | pz.setOutput(0, ana0/10) 12 | pz.setPixel(0,0,0,ana0/4) 13 | pz.setOutput(2, int(ana0/7)) 14 | time.sleep(0.1) # this makes 11 active lines, but can be removed 15 | 16 | -------------------------------------------------------------------------------- /Python/buttonTest.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # GNU GPL V3 4 | # Test code for 4tronix Picon Zero 5 | 6 | from __future__ import print_function 7 | import piconzero as pz, time 8 | 9 | pz.init() 10 | 11 | pz.setInputConfig(0, 0, True) # request pullup on input 12 | 13 | vsn = pz.getRevision() 14 | if (vsn[1] == 2): 15 | print("Board Type:", "Picon Zero") 16 | else: 17 | print("Board Type:", vsn[1]) 18 | print("Firmware version:", vsn[0]) 19 | print 20 | 21 | try: 22 | while True: 23 | switch = pz.readInput(0) 24 | if (switch == 0): 25 | print("Switch Pressed", switch) 26 | else: 27 | print("Switch Released", switch) 28 | time.sleep(5) 29 | except KeyboardInterrupt: 30 | print 31 | finally: 32 | pz.cleanup() 33 | -------------------------------------------------------------------------------- /Python/hcsr04.py: -------------------------------------------------------------------------------- 1 | #====================================================================== 2 | # 3 | # Python Module to handle an HC-SR04 Ultrasonic Module on a single Pin 4 | # Aimed at use on Picon Zero 5 | # 6 | # Created by Gareth Davies, Mar 2016 7 | # Copyright 4tronix 8 | # 9 | # This code is in the public domain and may be freely copied and used 10 | # No warranty is provided or implied 11 | # 12 | #====================================================================== 13 | 14 | import RPi.GPIO as GPIO, sys, threading, time, os, subprocess 15 | 16 | 17 | # Define Sonar Pin (Uses same pin for both Ping and Echo) 18 | sonar = 38 19 | 20 | #====================================================================== 21 | # General Functions 22 | # 23 | def init(): 24 | GPIO.setwarnings(False) 25 | GPIO.setmode(GPIO.BOARD) 26 | 27 | def cleanup(): 28 | GPIO.cleanup() 29 | 30 | #====================================================================== 31 | 32 | #====================================================================== 33 | # UltraSonic Functions 34 | # 35 | # getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object 36 | def getDistance(): 37 | GPIO.setup(sonar, GPIO.OUT) 38 | # Send 10us pulse to trigger 39 | GPIO.output(sonar, True) 40 | time.sleep(0.00001) 41 | GPIO.output(sonar, False) 42 | start = time.time() 43 | count=time.time() 44 | GPIO.setup(sonar,GPIO.IN) 45 | while GPIO.input(sonar)==0 and time.time()-count<0.1: 46 | start = time.time() 47 | count=time.time() 48 | stop=count 49 | while GPIO.input(sonar)==1 and time.time()-count<0.1: 50 | stop = time.time() 51 | # Calculate pulse length 52 | elapsed = stop-start 53 | # Distance pulse travelled in that time is time 54 | # multiplied by the speed of sound (cm/s) 55 | distance = elapsed * 34000 56 | # That was the distance there and back so halve the value 57 | distance = distance / 2 58 | return distance 59 | 60 | # End of UltraSonic Functions 61 | #====================================================================== 62 | 63 | -------------------------------------------------------------------------------- /Python/ioTest.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # GNU GPL V3 4 | # Test code for 4tronix Picon Zero 5 | 6 | from __future__ import print_function 7 | import piconzero as pz, time 8 | 9 | lastPix = 0 10 | numpixels = 8 11 | 12 | pz.init() 13 | pz.setInputConfig(0, 1) # set input 0 to Analog 14 | pz.setOutputConfig(0, 1) # set output 0 to PWM 15 | pz.setOutputConfig(2, 2) # set output 2 to Servo 16 | pz.setOutputConfig(5, 3) # set output 5 to WS2812 17 | rev = pz.getRevision() 18 | print(rev[0], rev[1]) 19 | try: 20 | while True: 21 | ana0 = pz.readInput(0) 22 | print (ana0, int(0.5 + ana0/113.7), int(ana0/7)) 23 | pz.setOutput(0, ana0/10) 24 | #setMotor(0, ana0/10) 25 | if (int(0.4 + ana0*numpixels/1000) != lastPix): 26 | lastPix = int(0.5 + ana0*numpixels/1000) 27 | for i in range (64): 28 | if i < lastPix: 29 | pz.setPixel(i, 0, 0, 255, False) 30 | else: 31 | pz.setPixel(i, 0, 0, 0, False) 32 | pz.updatePixels() 33 | pz.setOutput(2, int(ana0/7)) 34 | time.sleep(0.1) 35 | except KeyboardInterrupt: 36 | print() 37 | finally: 38 | pz.cleanup() 39 | -------------------------------------------------------------------------------- /Python/motorTest.py: -------------------------------------------------------------------------------- 1 | # Picon Zero Motor Test 2 | # Moves: Forward, Reverse, turn Right, turn Left, Stop - then repeat 3 | # Press Ctrl-C to stop 4 | # 5 | # To check wiring is correct ensure the order of movement as above is correct 6 | 7 | from __future__ import print_function 8 | import piconzero as pz, time 9 | 10 | #====================================================================== 11 | # Reading single character by forcing stdin to raw mode 12 | import sys 13 | import tty 14 | import termios 15 | 16 | def readchar(): 17 | fd = sys.stdin.fileno() 18 | old_settings = termios.tcgetattr(fd) 19 | try: 20 | tty.setraw(sys.stdin.fileno()) 21 | ch = sys.stdin.read(1) 22 | finally: 23 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 24 | if ch == '0x03': 25 | raise KeyboardInterrupt 26 | return ch 27 | 28 | def readkey(getchar_fn=None): 29 | getchar = getchar_fn or readchar 30 | c1 = getchar() 31 | if ord(c1) != 0x1b: 32 | return c1 33 | c2 = getchar() 34 | if ord(c2) != 0x5b: 35 | return c1 36 | c3 = getchar() 37 | return chr(0x10 + ord(c3) - 65) # 16=Up, 17=Down, 18=Right, 19=Left arrows 38 | 39 | # End of single character reading 40 | #====================================================================== 41 | 42 | speed = 60 43 | 44 | print("Tests the motors by using the arrow keys to control") 45 | print("Use , or < to slow down") 46 | print("Use . or > to speed up") 47 | print("Speed changes take effect when the next arrow key is pressed") 48 | print("Press Ctrl-C to end") 49 | print() 50 | 51 | pz.init() 52 | 53 | # main loop 54 | try: 55 | while True: 56 | keyp = readkey() 57 | if keyp == 'w' or ord(keyp) == 16: 58 | pz.forward(speed) 59 | print('Forward', speed) 60 | elif keyp == 'z' or ord(keyp) == 17: 61 | pz.reverse(speed) 62 | print('Reverse', speed) 63 | elif keyp == 's' or ord(keyp) == 18: 64 | pz.spinRight(speed) 65 | print('Spin Right', speed) 66 | elif keyp == 'a' or ord(keyp) == 19: 67 | pz.spinLeft(speed) 68 | print('Spin Left', speed) 69 | elif keyp == '.' or keyp == '>': 70 | speed = min(100, speed+10) 71 | print('Speed+', speed) 72 | elif keyp == ',' or keyp == '<': 73 | speed = max (0, speed-10) 74 | print('Speed-', speed) 75 | elif keyp == ' ': 76 | pz.stop() 77 | print('Stop') 78 | elif ord(keyp) == 3: 79 | break 80 | 81 | except KeyboardInterrupt: 82 | print 83 | 84 | finally: 85 | pz.cleanup() 86 | -------------------------------------------------------------------------------- /Python/piconzero.py: -------------------------------------------------------------------------------- 1 | # Python library for 4tronix Picon Zero 2 | # Note that all I2C accesses are wrapped in try clauses with repeats 3 | 4 | from __future__ import print_function 5 | import smbus, time 6 | 7 | bus = smbus.SMBus(1) # For revision 1 Raspberry Pi, change to bus = smbus.SMBus(0) 8 | 9 | pzaddr = 0x22 # I2C address of Picon Zero 10 | 11 | #--------------------------------------------- 12 | # Definitions of Commands to Picon Zero 13 | MOTORA = 0 14 | OUTCFG0 = 2 15 | OUTPUT0 = 8 16 | INCFG0 = 14 17 | SETBRIGHT = 18 18 | UPDATENOW = 19 19 | RESET = 20 20 | INPERIOD0 = 21 21 | #--------------------------------------------- 22 | 23 | #--------------------------------------------- 24 | # General variables 25 | DEBUG = False 26 | RETRIES = 10 # max number of retries for I2C calls 27 | #--------------------------------------------- 28 | 29 | #--------------------------------------------- 30 | # Get Version and Revision info 31 | def getRevision(): 32 | for i in range(RETRIES): 33 | try: 34 | rval = bus.read_word_data (pzaddr, 0) 35 | return [rval//256, rval%256] 36 | except: 37 | if (DEBUG): 38 | print("Error in getRevision(), retrying") 39 | #--------------------------------------------- 40 | 41 | 42 | #--------------------------------------------- 43 | # motor must be in range 0..1 44 | # value must be in range -128 - +127 45 | # values of -127, -128, +127 are treated as always ON,, no PWM 46 | def setMotor (motor, value): 47 | if (motor>=0 and motor<=1 and value>=-128 and value<128): 48 | for i in range(RETRIES): 49 | try: 50 | bus.write_byte_data (pzaddr, motor, value) 51 | break 52 | except: 53 | if (DEBUG): 54 | print("Error in setMotor(), retrying") 55 | 56 | def forward (speed): 57 | setMotor (0, speed) 58 | setMotor (1, speed) 59 | 60 | def reverse (speed): 61 | setMotor (0, -speed) 62 | setMotor (1, -speed) 63 | 64 | def spinLeft (speed): 65 | setMotor (0, -speed) 66 | setMotor (1, speed) 67 | 68 | def spinRight (speed): 69 | setMotor (0, speed) 70 | setMotor (1, -speed) 71 | 72 | def stop(): 73 | setMotor (0, 0) 74 | setMotor (1, 0) 75 | 76 | #--------------------------------------------- 77 | 78 | #--------------------------------------------- 79 | # Read data for selected input channel (analog or digital) 80 | # Channel is in range 0 to 3 81 | def readInput (channel): 82 | if (channel>=0 and channel <=3): 83 | for i in range(RETRIES): 84 | try: 85 | return bus.read_word_data (pzaddr, channel + 1) 86 | except: 87 | if (DEBUG): 88 | print("Error in readChannel(), retrying") 89 | 90 | #--------------------------------------------- 91 | 92 | #--------------------------------------------- 93 | # Set configuration of selected output 94 | # 0: On/Off, 1: PWM, 2: Servo, 3: WS2812B 95 | def setOutputConfig (output, value): 96 | if (output>=0 and output<=5 and value>=0 and value<=3): 97 | for i in range(RETRIES): 98 | try: 99 | bus.write_byte_data (pzaddr, OUTCFG0 + output, value) 100 | break 101 | except: 102 | if (DEBUG): 103 | print("Error in setOutputConfig(), retrying") 104 | #--------------------------------------------- 105 | 106 | #--------------------------------------------- 107 | # Set configuration of selected input channel 108 | # 0: Digital, 1: Analog, 2: DS18B20, 4: DutyCycle 5: Pulse Width 109 | def setInputConfig (channel, value, pullup = False, period = 2000): 110 | if (channel >= 0 and channel <= 3 and value >= 0 and value <= 5): 111 | if (value == 0 and pullup == True): 112 | value = 128 113 | for i in range(RETRIES): 114 | try: 115 | bus.write_byte_data (pzaddr, INCFG0 + channel, value) 116 | if (value == 4 or value == 5): 117 | bus.write_word_data (pzaddr, INPERIOD0 + channel, period) 118 | break 119 | except: 120 | if (DEBUG): 121 | print("Error in setInputConfig(), retrying") 122 | #--------------------------------------------- 123 | 124 | #--------------------------------------------- 125 | # Set output data for selected output channel 126 | # Mode Name Type Values 127 | # 0 On/Off Byte 0 is OFF, 1 is ON 128 | # 1 PWM Byte 0 to 100 percentage of ON time 129 | # 2 Servo Byte -100 to + 100 Position in degrees 130 | # 3 WS2812B 4 Bytes 0:Pixel ID, 1:Red, 2:Green, 3:Blue 131 | def setOutput (channel, value): 132 | if (channel>=0 and channel<=5): 133 | for i in range(RETRIES): 134 | try: 135 | bus.write_byte_data (pzaddr, OUTPUT0 + channel, value) 136 | break 137 | except: 138 | if (DEBUG): 139 | print("Error in setOutput(), retrying") 140 | #--------------------------------------------- 141 | 142 | #--------------------------------------------- 143 | # Set the colour of an individual pixel (always output 5) 144 | def setPixel (Pixel, Red, Green, Blue, Update=True): 145 | pixelData = [Pixel, Red, Green, Blue] 146 | for i in range(RETRIES): 147 | try: 148 | bus.write_i2c_block_data (pzaddr, Update, pixelData) 149 | break 150 | except: 151 | if (DEBUG): 152 | print("Error in setPixel(), retrying") 153 | 154 | def setAllPixels (Red, Green, Blue, Update=True): 155 | pixelData = [100, Red, Green, Blue] 156 | for i in range(RETRIES): 157 | try: 158 | bus.write_i2c_block_data (pzaddr, Update, pixelData) 159 | break 160 | except: 161 | if (DEBUG): 162 | print("Error in setAllPixels(), retrying") 163 | 164 | def updatePixels (): 165 | for i in range(RETRIES): 166 | try: 167 | bus.write_byte_data (pzaddr, UPDATENOW, 0) 168 | break 169 | except: 170 | if (DEBUG): 171 | print("Error in updatePixels(), retrying") 172 | 173 | #--------------------------------------------- 174 | 175 | #--------------------------------------------- 176 | # Set the overall brightness of pixel array 177 | def setBrightness (brightness): 178 | for i in range(RETRIES): 179 | try: 180 | bus.write_byte_data (pzaddr, SETBRIGHT, brightness) 181 | break 182 | except: 183 | if (DEBUG): 184 | print("Error in setBrightness(), retrying") 185 | #--------------------------------------------- 186 | 187 | #--------------------------------------------- 188 | # Initialise the Board (same as cleanup) 189 | def init (debug=False): 190 | DEBUG = debug 191 | for i in range(RETRIES): 192 | try: 193 | bus.write_byte_data (pzaddr, RESET, 0) 194 | break 195 | except: 196 | if (DEBUG): 197 | print("Error in init(), retrying") 198 | time.sleep(0.01) #1ms delay to allow time to complete 199 | if (DEBUG): 200 | print("Debug is", DEBUG) 201 | #--------------------------------------------- 202 | 203 | #--------------------------------------------- 204 | # Cleanup the Board (same as init) 205 | def cleanup (): 206 | for i in range(RETRIES): 207 | try: 208 | bus.write_byte_data (pzaddr, RESET, 0) 209 | break 210 | except: 211 | if (DEBUG): 212 | print("Error in cleanup(), retrying") 213 | time.sleep(0.001) # 1ms delay to allow time to complete 214 | #--------------------------------------------- 215 | -------------------------------------------------------------------------------- /Python/pixelTest.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # GNU GPL V3 4 | # Test code for 4tronix Picon Zero 5 | 6 | from __future__ import print_function 7 | import piconzero as pz, time 8 | 9 | pz.init() 10 | pz.setOutputConfig(5, 3) # set output 5 to WS2812 11 | rev = pz.getRevision() 12 | print(rev[0], rev[1]) 13 | try: 14 | while True: 15 | pz.setAllPixels(255,255,255) 16 | time.sleep(1) 17 | pz.setAllPixels(0,0,0) 18 | time.sleep(1) 19 | except KeyboardInterrupt: 20 | print() 21 | finally: 22 | pz.cleanup() 23 | -------------------------------------------------------------------------------- /Python/servoTest.py: -------------------------------------------------------------------------------- 1 | # Picon Zero Servo Test 2 | # Use arrow keys to move 2 servos on outputs 0 and 1 for Pan and Tilt 3 | # Use G and H to open and close the Gripper arm 4 | # Press Ctrl-C to stop 5 | # 6 | 7 | from __future__ import print_function 8 | import piconzero as pz, time 9 | 10 | #====================================================================== 11 | # Reading single character by forcing stdin to raw mode 12 | import sys 13 | import tty 14 | import termios 15 | 16 | def readchar(): 17 | fd = sys.stdin.fileno() 18 | old_settings = termios.tcgetattr(fd) 19 | try: 20 | tty.setraw(sys.stdin.fileno()) 21 | ch = sys.stdin.read(1) 22 | finally: 23 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 24 | if ch == '0x03': 25 | raise KeyboardInterrupt 26 | return ch 27 | 28 | def readkey(getchar_fn=None): 29 | getchar = getchar_fn or readchar 30 | c1 = getchar() 31 | if ord(c1) != 0x1b: 32 | return c1 33 | c2 = getchar() 34 | if ord(c2) != 0x5b: 35 | return c1 36 | c3 = getchar() 37 | return chr(0x10 + ord(c3) - 65) # 16=Up, 17=Down, 18=Right, 19=Left arrows 38 | 39 | # End of single character reading 40 | #====================================================================== 41 | 42 | speed = 60 43 | 44 | print("Tests the servos by using the arrow keys to control") 45 | print("Press key to centre") 46 | print("Press Ctrl-C to end") 47 | print() 48 | 49 | # Define which pins are the servos 50 | pan = 0 51 | tilt = 1 52 | grip = 2 53 | 54 | pz.init() 55 | 56 | # Set output mode to Servo 57 | pz.setOutputConfig(pan, 2) 58 | pz.setOutputConfig(tilt, 2) 59 | pz.setOutputConfig(grip, 2) 60 | 61 | # Centre all servos 62 | panVal = 90 63 | tiltVal = 90 64 | gripVal = 90 65 | pz.setOutput (pan, panVal) 66 | pz.setOutput (tilt, tiltVal) 67 | pz.setOutput (grip, gripVal) 68 | 69 | # main loop 70 | try: 71 | while True: 72 | keyp = readkey() 73 | if keyp == 'w' or ord(keyp) == 16: 74 | panVal = max (0, panVal - 5) 75 | print('Up', panVal) 76 | elif keyp == 'z' or ord(keyp) == 17: 77 | panVal = min (180, panVal + 5) 78 | print('Down', panVal) 79 | elif keyp == 's' or ord(keyp) == 18: 80 | tiltVal = max (0, tiltVal - 5) 81 | print('Right', tiltVal) 82 | elif keyp == 'a' or ord(keyp) == 19: 83 | tiltVal = min (180, tiltVal + 5) 84 | print('Left', tiltVal) 85 | elif keyp == 'g': 86 | gripVal = max (0, gripVal - 5) 87 | print('Open', gripVal) 88 | elif keyp == 'h': 89 | gripVal = min (180, gripVal + 5) 90 | print('Close', gripVal) 91 | elif keyp == ' ': 92 | panVal = tiltVal = gripVal = 90 93 | print('Centre') 94 | elif ord(keyp) == 3: 95 | break 96 | pz.setOutput (pan, panVal) 97 | pz.setOutput (tilt, tiltVal) 98 | pz.setOutput (grip, gripVal) 99 | 100 | except KeyboardInterrupt: 101 | print() 102 | 103 | finally: 104 | pz.cleanup() 105 | -------------------------------------------------------------------------------- /Python/sonarTest.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # 3 | # Basic test of HC-SR04 ultrasonic sensor on Picon Zero 4 | 5 | from __future__ import print_function 6 | import hcsr04, time 7 | 8 | hcsr04.init() 9 | 10 | try: 11 | while True: 12 | distance = int(hcsr04.getDistance()) 13 | print("Distance:", distance) 14 | time.sleep(1) 15 | except KeyboardInterrupt: 16 | print() 17 | finally: 18 | hcsr04.cleanup() 19 | -------------------------------------------------------------------------------- /Python/tempTest.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # GNU GPL V3 4 | # Test code for 4tronix Picon Zero 5 | 6 | from __future__ import print_function 7 | import piconzero as pz, time 8 | 9 | pz.init() 10 | pz.setInputConfig(0, 2) # set input 0 to DS18B20 11 | pz.setInputConfig(2, 2) # set input 2 to DS18B20 12 | 13 | try: 14 | while True: 15 | ana0 = pz.readInput(0) 16 | if (ana0>32767): 17 | ana0 -= 65536 18 | ana2 = pz.readInput(2) 19 | if (ana2>32767): 20 | ana2 -= 65536 21 | print(ana0*0.0625, ana2*0.062) 22 | time.sleep(1) 23 | except KeyboardInterrupt: 24 | print() 25 | finally: 26 | pz.cleanup() 27 | -------------------------------------------------------------------------------- /Python/version.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # GNU GPL V3 4 | # Test code for 4tronix Picon Zero 5 | 6 | from __future__ import print_function 7 | import piconzero as pz 8 | 9 | pz.init() 10 | vsn = pz.getRevision() 11 | if (vsn[1] == 2): 12 | print("Board Type:", "Picon Zero") 13 | else: 14 | print("Board Type:", vsn[1]) 15 | print("Firmware version:", vsn[0]) 16 | print() 17 | pz.cleanup() 18 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | Picon Zero 2 | ======= 3 | Picon Zero is a Raspberry Pi addon in the form factor of a Pi Zero. It contains an ATMega328 with the Arduino bootloader. The ATMega328 is connected to the Raspberry Pi via I2C and handles all the interface to the I/O ports and motor drivers. 4 | Specifically, the ATMega328 allows: 5 | - Dual H-Bridge motor driver (DRV8833) 6 | - Standard digital outputs (On / Off) 7 | - PWM outputs (0 to 100%) 8 | - Servo outputs (-90 to +90 degrees) 9 | - WS2812B compatible smart pixels (up to 100 pixels) 10 | - Standard digital inputs (On / Off) 11 | - Analog inputs (0 to 255) 12 | 13 | Picon Zero Released Files 14 | ====================== 15 | All files are Licensed under Creative Commons BY-SA 16 | See creativecommons.org/licenses/by-sa/3.0/ for details 17 | - piconzero.py (this is the main Python library) 18 | - various python example files 19 | - PiconZero10.ino (this is the ATMega code) 20 | 21 | Programming the ATMega 22 | ====================== 23 | You can reprogram the ATMega with your own code by using a USB to Serial converter that has the following pinout: 24 | - DTR 25 | - RXD 26 | - TXD 27 | - VCC 28 | - N/A (usually CTS, but not used) 29 | - GND 30 | There are many sources of this converter including the 4tronix shop: https://shop.4tronix.co.uk/products/usb340g 31 | --------------------------------------------------------------------------------