├── Amaya ├── Readme.md └── piccolaMain │ ├── piccolaMazeSolve.h │ ├── piccolaVariables.h │ ├── piccolaPins.h │ ├── piccolaSpeed.h │ ├── piccolaMain.ino │ ├── piccolaWallPid.h │ ├── piccolaMotors.h │ ├── piccolaTurns.h │ └── piccolaTof.h ├── DXF ├── base1.SLDPRT ├── base2.SLDPRT └── base3.SLDPRT ├── piccolaAltium ├── piccolaAltium.PrjPcbStructure ├── piccolaPCB.PcbDoc ├── piccaloSchematic.SchDoc └── Project Logs for piccolaAltium │ ├── piccolaPCB PCB ECO 7-16-2019 9-00-02 PM.LOG │ └── piccolaPCB PCB ECO 7-16-2019 8-12-45 PM.LOG ├── piccolaPowerSupply ├── piccolaPowerSupply.PrjPcbStructure ├── piccolaPowerPCB.PcbDoc ├── piccolaPowerSupply.SchDoc └── Project Logs for piccolaPowerSupply │ └── piccolaPowerPCB PCB ECO 7-17-2019 10-42-54 PM.LOG ├── __pycache__ └── API.cpython-36.pyc ├── code ├── .vscode │ ├── arduino.json │ └── c_cpp_properties.json ├── piccolaZlgoSupport.h ├── flash_stm32.h ├── piccolaMain.ino ├── piccolaVariables.h ├── piccolaPins.h ├── piccolaMotors.h ├── EEPROM.h ├── piccolaSpeed.h ├── piccolaZlgoVariables.h ├── piccolaWallPid.h ├── piccolaTof.h ├── piccolaTurns.h ├── piccolaMoves.h ├── piccolaMazeSolve.h └── piccolaZlgo.h ├── piccolaMain ├── .vscode │ ├── arduino.json │ └── c_cpp_properties.json ├── piccolaZlgoSupport.h ├── flash_stm32.h ├── piccolaMain.ino ├── piccolaVariables.h ├── piccolaPins.h ├── piccolaMotors.h ├── EEPROM.h ├── piccolaZlgoVariables.h ├── piccolaSpeed.h ├── piccolaWallPid.h ├── piccolaTof.h ├── piccolaTurns.h ├── piccolaMoves.h ├── piccolaMazeSolve.h └── piccolaZlgo.h ├── README.md ├── arduinoAlgoCheck ├── piccolaZlgoSupport.h ├── arduinoAlgoCheck.ino ├── piccolaZlgoVariables.h ├── piccolaMazeSolve.h └── piccolaZlgo.h └── API.py /Amaya/Readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /DXF/base1.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/DXF/base1.SLDPRT -------------------------------------------------------------------------------- /DXF/base2.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/DXF/base2.SLDPRT -------------------------------------------------------------------------------- /DXF/base3.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/DXF/base3.SLDPRT -------------------------------------------------------------------------------- /piccolaAltium/piccolaAltium.PrjPcbStructure: -------------------------------------------------------------------------------- 1 | Record=TopLevelDocument|FileName=piccaloSchematic.SchDoc 2 | -------------------------------------------------------------------------------- /piccolaPowerSupply/piccolaPowerSupply.PrjPcbStructure: -------------------------------------------------------------------------------- 1 | Record=TopLevelDocument|FileName=piccolaPowerSupply.SchDoc 2 | -------------------------------------------------------------------------------- /__pycache__/API.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/__pycache__/API.cpython-36.pyc -------------------------------------------------------------------------------- /piccolaAltium/piccolaPCB.PcbDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/piccolaAltium/piccolaPCB.PcbDoc -------------------------------------------------------------------------------- /piccolaAltium/piccaloSchematic.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/piccolaAltium/piccaloSchematic.SchDoc -------------------------------------------------------------------------------- /piccolaPowerSupply/piccolaPowerPCB.PcbDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/piccolaPowerSupply/piccolaPowerPCB.PcbDoc -------------------------------------------------------------------------------- /piccolaPowerSupply/piccolaPowerSupply.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Isuru-Dissanayake/piccola/HEAD/piccolaPowerSupply/piccolaPowerSupply.SchDoc -------------------------------------------------------------------------------- /code/.vscode/arduino.json: -------------------------------------------------------------------------------- 1 | { 2 | "port": "COM1", 3 | "board": "stm32duino:STM32F1:genericSTM32F103C", 4 | "programmer": "ArduinoISP", 5 | "sketch": "piccolaMain.ino", 6 | "configuration": "device_variant=STM32F103C8,upload_method=STLinkMethod,cpu_speed=speed_72mhz,opt=osstd" 7 | } -------------------------------------------------------------------------------- /piccolaMain/.vscode/arduino.json: -------------------------------------------------------------------------------- 1 | { 2 | "port": "COM1", 3 | "board": "stm32duino:STM32F1:genericSTM32F103C", 4 | "programmer": "ArduinoISP", 5 | "sketch": "piccolaMain.ino", 6 | "configuration": "device_variant=STM32F103C8,upload_method=STLinkMethod,cpu_speed=speed_72mhz,opt=osstd" 7 | } -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaMazeSolve.h: -------------------------------------------------------------------------------- 1 | void mazeSolve() 2 | { 3 | switch (nextMove) 4 | { 5 | case 'F' : 6 | //move forward 7 | break; 8 | 9 | case 'R' : 10 | //move right 11 | break; 12 | 13 | case 'L' : 14 | //move left 15 | break; 16 | 17 | case 'B' : 18 | //move back 19 | break; 20 | } 21 | } -------------------------------------------------------------------------------- /piccolaAltium/Project Logs for piccolaAltium/piccolaPCB PCB ECO 7-16-2019 9-00-02 PM.LOG: -------------------------------------------------------------------------------- 1 | Change Component DesignItemId : Designator=stm32 Old DesignItemId=BLUE_PILLADVANCED New DesignItemId=BLUE_PILL 2 | Change Component Footprint: Designator=stm32 Old Footprint=BLUE_PILL_ADV New Footprint=BLUE_PILL 3 | Change Component Comment : Designator=stm32 Old Comment=BLUE_PILLADVANCED New Comment= 4 | Change Component Library : Designator=stm32 Old Library=BluePill.SCHLIB New Library=BluePill.IntLib 5 | Added Member To Class: ClassName=piccaloSchematic Member=Component tof03 Header 7 6 | Added Room: Name=piccaloSchematic 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # piccola 2 | This repository contains all the codes used in building the micromouse named "Piccola", to participate in the Robofest 2019 competion in Sri lanka. 3 | You can find codes for following functionalities as header files inside the 'code' folder 4 | - interfaceing multiple VL6180X with stm32 5 | - forward, turns, brake, acceleration functions 6 | - debuging using HC06 bluetooth module 7 | - flood fill algorithm and shortest path calculating 8 | - reading writing with eeprom stm32 9 | - piccola maze solving stratergy 10 | 11 | Stay tuned for a article series on medium about piccola! 12 | -------------------------------------------------------------------------------- /arduinoAlgoCheck/piccolaZlgoSupport.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | void showCells(){ 4 | for (int i=13; i>=0; i--){ 5 | for (int j=0; j<14;j++){ 6 | Serial.print(cells[i][j]); 7 | Serial.print(' '); 8 | } 9 | Serial.println(' '); 10 | } 11 | } 12 | 13 | void showFlood(){ 14 | for (int i=13; i>=0; i--){ 15 | for (int j=0; j<14;j++){ 16 | Serial.print(flood[i][j]); 17 | Serial.print(' '); 18 | } 19 | Serial.println(' '); 20 | } 21 | } 22 | 23 | void showFlood2(){ 24 | for (int i=13; i>=0; i--){ 25 | for (int j=0; j<14;j++){ 26 | Serial.print(flood2[i][j]); 27 | Serial.print(' '); 28 | } 29 | Serial.println(' '); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /code/piccolaZlgoSupport.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | void showCells(){ 4 | for (int i=13; i>=0; i--){ 5 | for (int j=0; j<14;j++){ 6 | Serial.print(cells[i][j]); 7 | Serial.print(' '); 8 | } 9 | Serial.println(' '); 10 | } 11 | } 12 | 13 | void showFlood(){ 14 | for (int i=13; i>=0; i--){ 15 | for (int j=0; j<14;j++){ 16 | Serial.print(flood[i][j]); 17 | Serial.print(' '); 18 | } 19 | Serial.println(' '); 20 | } 21 | } 22 | 23 | void showFlood2(){ 24 | for (int i=13; i>=0; i--){ 25 | for (int j=0; j<14;j++){ 26 | Serial.print(flood2[i][j]); 27 | Serial.print(' '); 28 | } 29 | Serial.println(' '); 30 | } 31 | } 32 | 33 | */ 34 | -------------------------------------------------------------------------------- /piccolaMain/piccolaZlgoSupport.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | void showCells(){ 4 | for (int i=13; i>=0; i--){ 5 | for (int j=0; j<14;j++){ 6 | Serial.print(cells[i][j]); 7 | Serial.print(' '); 8 | } 9 | Serial.println(' '); 10 | } 11 | } 12 | 13 | void showFlood(){ 14 | for (int i=13; i>=0; i--){ 15 | for (int j=0; j<14;j++){ 16 | Serial.print(flood[i][j]); 17 | Serial.print(' '); 18 | } 19 | Serial.println(' '); 20 | } 21 | } 22 | 23 | void showFlood2(){ 24 | for (int i=13; i>=0; i--){ 25 | for (int j=0; j<14;j++){ 26 | Serial.print(flood2[i][j]); 27 | Serial.print(' '); 28 | } 29 | Serial.println(' '); 30 | } 31 | } 32 | 33 | */ 34 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaVariables.h: -------------------------------------------------------------------------------- 1 | int leftBase =180; 2 | int rightBase = 177; 3 | int leftPwm; 4 | int rightPwm; 5 | 6 | int leftEncoder; 7 | int rightEncoder; 8 | int encoderLeftCount; 9 | int encoderRightCount; 10 | 11 | float tof[5]; 12 | int wallAvailable[2]; 13 | int cellWalls[3]; 14 | 15 | float leftP = 0.15; 16 | float leftD = 0; 17 | 18 | float leftError = 0; 19 | float leftLastError = 0; 20 | 21 | float rightP = 0.15; 22 | float rightD = 0; 23 | 24 | float rightError = 0; 25 | float rightLastError = 0; 26 | 27 | float wallP = 0.15; 28 | float wallD = 0; 29 | 30 | float wallError = 0; 31 | float wallLastError = 0; 32 | 33 | float correction; 34 | 35 | char nextMove = 'F'; 36 | //String Val= 'ab'; 37 | -------------------------------------------------------------------------------- /code/flash_stm32.h: -------------------------------------------------------------------------------- 1 | #ifndef __FLASH_STM32_H 2 | #define __FLASH_STM32_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | typedef enum 9 | { 10 | FLASH_BUSY = 1, 11 | FLASH_ERROR_PG, 12 | FLASH_ERROR_WRP, 13 | FLASH_ERROR_OPT, 14 | FLASH_COMPLETE, 15 | FLASH_TIMEOUT, 16 | FLASH_BAD_ADDRESS 17 | } FLASH_Status; 18 | 19 | #define IS_FLASH_ADDRESS(ADDRESS) (((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x0807FFFF)) 20 | 21 | FLASH_Status FLASH_WaitForLastOperation(uint32 Timeout); 22 | FLASH_Status FLASH_ErasePage(uint32 Page_Address); 23 | FLASH_Status FLASH_ProgramHalfWord(uint32 Address, uint16 Data); 24 | 25 | void FLASH_Unlock(void); 26 | void FLASH_Lock(void); 27 | 28 | #ifdef __cplusplus 29 | } 30 | #endif 31 | 32 | #endif /* __FLASH_STM32_H */ 33 | -------------------------------------------------------------------------------- /piccolaMain/flash_stm32.h: -------------------------------------------------------------------------------- 1 | #ifndef __FLASH_STM32_H 2 | #define __FLASH_STM32_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | typedef enum 9 | { 10 | FLASH_BUSY = 1, 11 | FLASH_ERROR_PG, 12 | FLASH_ERROR_WRP, 13 | FLASH_ERROR_OPT, 14 | FLASH_COMPLETE, 15 | FLASH_TIMEOUT, 16 | FLASH_BAD_ADDRESS 17 | } FLASH_Status; 18 | 19 | #define IS_FLASH_ADDRESS(ADDRESS) (((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x0807FFFF)) 20 | 21 | FLASH_Status FLASH_WaitForLastOperation(uint32 Timeout); 22 | FLASH_Status FLASH_ErasePage(uint32 Page_Address); 23 | FLASH_Status FLASH_ProgramHalfWord(uint32 Address, uint16 Data); 24 | 25 | void FLASH_Unlock(void); 26 | void FLASH_Lock(void); 27 | 28 | #ifdef __cplusplus 29 | } 30 | #endif 31 | 32 | #endif /* __FLASH_STM32_H */ 33 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaPins.h: -------------------------------------------------------------------------------- 1 | #define PWMA PB8 //motor driver pins 2 | #define AIN2 PA12 3 | #define AIN1 PA7 4 | #define STBY PA11 5 | #define BIN1 PB0 6 | #define BIN2 PB1 7 | #define PWMB PB9 8 | 9 | #define leftOut1 PB12 //interrupt pins of motors 10 | #define leftOut2 PB13 11 | #define rightOut1 PB14 12 | #define rightOut2 PB15 13 | 14 | 15 | void motorDiver() 16 | { 17 | pinMode(PWMA, OUTPUT); 18 | pinMode(AIN2, OUTPUT); 19 | pinMode(AIN1, OUTPUT); 20 | pinMode(STBY, OUTPUT); 21 | pinMode(BIN1, OUTPUT); 22 | pinMode(BIN2, OUTPUT); 23 | pinMode(PWMB, OUTPUT); 24 | } 25 | 26 | void motorInterrupt() 27 | { 28 | pinMode(leftOut1, INPUT); 29 | pinMode(leftOut2, INPUT); 30 | pinMode(rightOut1, INPUT); 31 | pinMode(rightOut2, INPUT); 32 | } 33 | -------------------------------------------------------------------------------- /code/piccolaMain.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "piccolaPins.h" 5 | #include "piccolaZlgoVariables.h" 6 | #include "piccolaVariables.h" 7 | #include "piccolaTof.h" 8 | #include "piccolaMotors.h" 9 | #include "piccolaTurns.h" 10 | #include "piccolaWallPid.h" 11 | #include "piccolaSpeed.h" 12 | #include "piccolaMoves.h" 13 | #include "piccolaZlgo.h" 14 | #include "piccolaMazeSolve.h" 15 | 16 | void setup() 17 | { stbyHigh(); 18 | tofSetup(); 19 | motorDiver(); 20 | motorInterrupt(); 21 | pinMode(buzzer, OUTPUT); 22 | attachInterrupt(digitalPinToInterrupt(PB12), countLeftOut1, RISING); 23 | attachInterrupt(digitalPinToInterrupt(PB13), countLeftOut1, RISING); 24 | attachInterrupt(digitalPinToInterrupt(PB14), countRightOut1, RISING); 25 | attachInterrupt(digitalPinToInterrupt(PB15), countRightOut1, RISING); 26 | Serial2.begin(9600); 27 | } 28 | 29 | 30 | void loop() 31 | { 32 | delay(1000); 33 | searchStates(); 34 | //eepromClear(); 35 | } 36 | -------------------------------------------------------------------------------- /piccolaMain/piccolaMain.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "piccolaPins.h" 5 | #include "piccolaZlgoVariables.h" 6 | #include "piccolaVariables.h" 7 | #include "piccolaTof.h" 8 | #include "piccolaMotors.h" 9 | #include "piccolaTurns.h" 10 | #include "piccolaWallPid.h" 11 | #include "piccolaSpeed.h" 12 | #include "piccolaMoves.h" 13 | #include "piccolaZlgo.h" 14 | #include "piccolaMazeSolve.h" 15 | 16 | void setup() 17 | { stbyHigh(); 18 | tofSetup(); 19 | motorDiver(); 20 | motorInterrupt(); 21 | pinMode(buzzer, OUTPUT); 22 | attachInterrupt(digitalPinToInterrupt(PB12), countLeftOut1, RISING); 23 | attachInterrupt(digitalPinToInterrupt(PB13), countLeftOut1, RISING); 24 | attachInterrupt(digitalPinToInterrupt(PB14), countRightOut1, RISING); 25 | attachInterrupt(digitalPinToInterrupt(PB15), countRightOut1, RISING); 26 | Serial2.begin(9600); 27 | } 28 | 29 | 30 | void loop() 31 | { 32 | delay(1000); 33 | searchStates(); 34 | //eepromClear(); 35 | } 36 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaSpeed.h: -------------------------------------------------------------------------------- 1 | void cellStart() 2 | {rightEncoder=0; 3 | leftEncoder=0; 4 | encoderRightCount=600; 5 | encoderLeftCount=600; 6 | rightBase=60; 7 | leftBase=60; 8 | 9 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 10 | { 11 | 12 | rightBase = int(70+110/(1+pow(2.73,((300-rightEncoder)*0.012)))); 13 | leftBase = int(70+110/(1+pow(2.73,((300-leftEncoder)*0.012)))); 14 | forwardBase(); 15 | } 16 | rightBase=175; 17 | leftBase=180; 18 | forwardBase(); 19 | } 20 | 21 | void cellBrake() 22 | { 23 | rightEncoder=0; 24 | leftEncoder=0; 25 | encoderRightCount=600; 26 | encoderLeftCount=600; 27 | rightBase=175; 28 | leftBase=180; 29 | 30 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 31 | { 32 | rightBase = int(180-110/(1+pow(2.73,((300-rightEncoder)*0.012)))); 33 | leftBase = int(180-110/(1+pow(2.73,((300-leftEncoder)*0.012)))); 34 | 35 | forwardBase(); 36 | 37 | } 38 | brake(); 39 | } 40 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaMain.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "piccolaPins.h" 4 | #include "piccolaVariables.h" 5 | #include "piccolaTof.h" 6 | #include "piccolaMotors.h" 7 | #include "piccolaTurns.h" 8 | #include "piccolaWallPid.h" 9 | #include "piccolaSpeed.h" 10 | 11 | 12 | 13 | void setup() 14 | { 15 | tofSetup(); 16 | motorDiver(); 17 | motorInterrupt(); 18 | attachInterrupt(digitalPinToInterrupt(PB12), countLeftOut1, RISING); 19 | attachInterrupt(digitalPinToInterrupt(PB13), countLeftOut2, RISING); 20 | attachInterrupt(digitalPinToInterrupt(PB14), countRightOut1, RISING); 21 | attachInterrupt(digitalPinToInterrupt(PB15), countRightOut2, RISING); 22 | Serial2.begin(9600); 23 | delay(1000); 24 | } 25 | 26 | void loop() 27 | { 28 | if(Serial2.available()) 29 | { 30 | Val=Serial2.read(); 31 | if (Val[0]=='p') 32 | { 33 | wallP= float(Val[1:4]); 34 | Serial2.println(wallP); 35 | } 36 | else if (Val[0]=='d') 37 | { 38 | wallD= float(Val[1:4]); 39 | Serial2.println(wallD); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /arduinoAlgoCheck/arduinoAlgoCheck.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "piccolaZlgoVariables.h" 3 | #include "piccolaZlgo.h" 4 | #include "piccolaZlgoSupport.h" 5 | #include "piccolaMazeSolve.h" 6 | 7 | 8 | void setup() 9 | { 10 | Serial2.begin(9600); 11 | } 12 | 13 | void loopt(){ 14 | appendDestination(0,13,false); 15 | floodFill3(); 16 | showFlood(); 17 | while(1){ 18 | } 19 | } 20 | 21 | void loopq(){ 22 | /* 23 | //traverse(byte xdes, byte ydes, boolean middleSquare, boolean shortPath) 24 | traverse(0,0,true,false); // traverses to center 25 | Serial.println(' '); 26 | Serial.println(' '); 27 | center(); //saves the walls of the middle square 28 | showCells(); 29 | Serial.println(' '); 30 | Serial.println(' '); 31 | 32 | 33 | traverse(0,0,false,false); // traverses back to the starting square 34 | Serial.println(' '); 35 | Serial.println(' '); 36 | showCells(); 37 | Serial.println(' '); 38 | Serial.println(' '); 39 | traverse(0,0,true,true); 40 | showCells(); 41 | Serial.println(' '); 42 | 43 | while(1){} 44 | */ 45 | } 46 | 47 | void loop(){ 48 | for (int i=0; i<100; i++){ 49 | EEPROM.write(i,'F'); 50 | } 51 | 52 | for(int i=0 ; i<100; i++){ 53 | Serial2.println(EEPROM.read(i)); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaWallPid.h: -------------------------------------------------------------------------------- 1 | void leftPid() 2 | { 3 | leftError = 60 - tof[0]; 4 | correction = (leftError * leftP) + ((leftError - leftLastError) * leftD); 5 | leftLastError = leftError; 6 | leftPwm = leftBase + correction; 7 | rightPwm = rightBase - correction; 8 | } 9 | 10 | void rightPid() 11 | { 12 | rightError = 60 - tof[4]; 13 | correction = (rightError * rightP) + ((rightError - rightLastError) * rightD); 14 | rightLastError = rightError; 15 | leftPwm = leftBase - correction; 16 | rightPwm = rightBase + correction; 17 | 18 | } 19 | 20 | void wallPid() 21 | { 22 | wallError = tof[0] - tof[4]; 23 | correction = (wallError * wallP) + ((wallError - wallLastError) * wallD); 24 | wallLastError = wallError; 25 | leftPwm = leftBase - correction; 26 | rightPwm = rightBase + correction; 27 | } 28 | 29 | void wallFollow() 30 | { 31 | checkWallsPid(); 32 | if (wallAvailable[0] == 1 && wallAvailable[1] == 1) 33 | { 34 | wallPid(); 35 | forward(); 36 | } 37 | 38 | else if (wallAvailable[0] == 0 && wallAvailable[1] == 1) 39 | { 40 | rightPid(); 41 | forward(); 42 | } 43 | 44 | else if (wallAvailable[0] == 1 && wallAvailable[1] == 0) 45 | { 46 | leftPid(); 47 | forward(); 48 | } 49 | } -------------------------------------------------------------------------------- /code/piccolaVariables.h: -------------------------------------------------------------------------------- 1 | int leftBase =180; 2 | int rightBase = 176; 3 | int leftPwm ; 4 | int rightPwm ; 5 | 6 | unsigned long leftEncoder = 0; 7 | unsigned long rightEncoder = 0; 8 | unsigned long encoderLeftCount = 0; 9 | unsigned long encoderRightCount = 0; 10 | 11 | int dif = 0; 12 | int test = 0; 13 | 14 | float tof[5]; 15 | int wallAvailable[3]; 16 | int cellWalls[3]; 17 | 18 | int leftWallAvailable = 0; 19 | int rightWallAvailable = 0; 20 | int frontWallAvailable = 0; 21 | 22 | int rightWall = 1; 23 | int leftWall = 1; 24 | 25 | float leftP = 0.8; 26 | float leftD = 3.4; 27 | 28 | float leftError = 0; 29 | float leftLastError = 0; 30 | float leftDiff = 0; 31 | 32 | float rightP = 0.8; 33 | float rightD = 3.4; 34 | 35 | float rightError = 0; 36 | float rightLastError = 0; 37 | float rightDiff = 0; 38 | 39 | float wallP = 0.8 ; 40 | float wallD = 2.7; 41 | 42 | float wallError = 0; 43 | float wallLastError = 0; 44 | float wallDiff = 0; 45 | 46 | float encoderP = 0.2 ; 47 | float encoderD = 0; 48 | 49 | float encoderError = 0; 50 | float encoderLastError = 0; 51 | float encoderDiff = 0; 52 | 53 | float encoderCorrection = 0; 54 | float correction; 55 | 56 | char nextMove = 'F'; 57 | 58 | int start = 0; 59 | 60 | int time; 61 | 62 | 63 | boolean left,right,front; 64 | int cell =0; 65 | 66 | int jump = 0; 67 | int preState = 0; 68 | int selectMode = -1; 69 | 70 | -------------------------------------------------------------------------------- /piccolaMain/piccolaVariables.h: -------------------------------------------------------------------------------- 1 | int leftBase =180; 2 | int rightBase = 176; 3 | int leftPwm ; 4 | int rightPwm ; 5 | 6 | unsigned long leftEncoder = 0; 7 | unsigned long rightEncoder = 0; 8 | unsigned long encoderLeftCount = 0; 9 | unsigned long encoderRightCount = 0; 10 | 11 | int dif = 0; 12 | int test = 0; 13 | 14 | float tof[5]; 15 | int wallAvailable[3]; 16 | int cellWalls[3]; 17 | 18 | int leftWallAvailable = 0; 19 | int rightWallAvailable = 0; 20 | int frontWallAvailable = 0; 21 | 22 | int rightWall = 1; 23 | int leftWall = 1; 24 | 25 | float leftP = 0.9; 26 | float leftD = 3.4; 27 | 28 | float leftError = 0; 29 | float leftLastError = 0; 30 | float leftDiff = 0; 31 | 32 | float rightP = 0.9; 33 | float rightD = 3.4; 34 | 35 | float rightError = 0; 36 | float rightLastError = 0; 37 | float rightDiff = 0; 38 | 39 | float wallP = 0.8 ; 40 | float wallD = 2.7; 41 | 42 | float wallError = 0; 43 | float wallLastError = 0; 44 | float wallDiff = 0; 45 | 46 | float encoderP = 0.2 ; 47 | float encoderD = 0; 48 | 49 | float encoderError = 0; 50 | float encoderLastError = 0; 51 | float encoderDiff = 0; 52 | 53 | float encoderCorrection = 0; 54 | float correction; 55 | 56 | char nextMove = 'F'; 57 | 58 | int start = 0; 59 | 60 | int time; 61 | 62 | 63 | boolean left,right,front; 64 | int cell =0; 65 | 66 | int jump = 0; 67 | int preState = 0; 68 | int selectMode = -1; 69 | 70 | -------------------------------------------------------------------------------- /code/piccolaPins.h: -------------------------------------------------------------------------------- 1 | #define PWMA PB8 //motor driver pins 2 | #define AIN2 PA12 3 | #define AIN1 PA7 4 | #define STBY PA11 5 | #define BIN1 PB0 6 | #define BIN2 PB1 7 | #define PWMB PB9 8 | 9 | #define leftOut1 PB12 //interrupt pins of motors 10 | #define leftOut2 PB13 11 | #define rightOut1 PB14 12 | #define rightOut2 PB15 13 | 14 | #define buzzer PB11 15 | 16 | void motorDiver() 17 | { 18 | pinMode(PWMA, OUTPUT); 19 | pinMode(AIN2, OUTPUT); 20 | pinMode(AIN1, OUTPUT); 21 | pinMode(STBY, OUTPUT); 22 | pinMode(BIN1, OUTPUT); 23 | pinMode(BIN2, OUTPUT); 24 | pinMode(PWMB, OUTPUT); 25 | } 26 | 27 | void motorInterrupt() 28 | { 29 | pinMode(leftOut1, INPUT); 30 | pinMode(leftOut2, INPUT); 31 | pinMode(rightOut1, INPUT); 32 | pinMode(rightOut2, INPUT); 33 | } 34 | 35 | void buzz() 36 | { 37 | digitalWrite(buzzer, HIGH); 38 | delay(200); 39 | digitalWrite(buzzer, LOW); 40 | delay(200); 41 | } 42 | 43 | void buzzTwice() 44 | { 45 | digitalWrite(buzzer, HIGH); 46 | delay(75); 47 | digitalWrite(buzzer, LOW); 48 | delay(50); 49 | digitalWrite(buzzer, HIGH); 50 | delay(100); 51 | digitalWrite(buzzer, LOW); 52 | } 53 | 54 | void buzzDone() 55 | { 56 | digitalWrite(buzzer, HIGH); 57 | delay(75); 58 | digitalWrite(buzzer, LOW); 59 | delay(30); 60 | digitalWrite(buzzer, HIGH); 61 | delay(75); 62 | digitalWrite(buzzer, LOW); 63 | delay(30); 64 | digitalWrite(buzzer, HIGH); 65 | delay(150); 66 | digitalWrite(buzzer, LOW); 67 | } -------------------------------------------------------------------------------- /piccolaMain/piccolaPins.h: -------------------------------------------------------------------------------- 1 | #define PWMA PB8 //motor driver pins 2 | #define AIN2 PA12 3 | #define AIN1 PA7 4 | #define STBY PA11 5 | #define BIN1 PB0 6 | #define BIN2 PB1 7 | #define PWMB PB9 8 | 9 | #define leftOut1 PB12 //interrupt pins of motors 10 | #define leftOut2 PB13 11 | #define rightOut1 PB14 12 | #define rightOut2 PB15 13 | 14 | #define buzzer PB11 15 | 16 | void motorDiver() 17 | { 18 | pinMode(PWMA, OUTPUT); 19 | pinMode(AIN2, OUTPUT); 20 | pinMode(AIN1, OUTPUT); 21 | pinMode(STBY, OUTPUT); 22 | pinMode(BIN1, OUTPUT); 23 | pinMode(BIN2, OUTPUT); 24 | pinMode(PWMB, OUTPUT); 25 | } 26 | 27 | void motorInterrupt() 28 | { 29 | pinMode(leftOut1, INPUT); 30 | pinMode(leftOut2, INPUT); 31 | pinMode(rightOut1, INPUT); 32 | pinMode(rightOut2, INPUT); 33 | } 34 | 35 | void buzz() 36 | { 37 | digitalWrite(buzzer, HIGH); 38 | delay(200); 39 | digitalWrite(buzzer, LOW); 40 | delay(200); 41 | } 42 | 43 | void buzzTwice() 44 | { 45 | digitalWrite(buzzer, HIGH); 46 | delay(75); 47 | digitalWrite(buzzer, LOW); 48 | delay(50); 49 | digitalWrite(buzzer, HIGH); 50 | delay(100); 51 | digitalWrite(buzzer, LOW); 52 | } 53 | 54 | void buzzDone() 55 | { 56 | digitalWrite(buzzer, HIGH); 57 | delay(75); 58 | digitalWrite(buzzer, LOW); 59 | delay(30); 60 | digitalWrite(buzzer, HIGH); 61 | delay(75); 62 | digitalWrite(buzzer, LOW); 63 | delay(30); 64 | digitalWrite(buzzer, HIGH); 65 | delay(150); 66 | digitalWrite(buzzer, LOW); 67 | } -------------------------------------------------------------------------------- /code/piccolaMotors.h: -------------------------------------------------------------------------------- 1 | void stbyHigh() 2 | { 3 | digitalWrite(STBY, HIGH); 4 | } 5 | 6 | void stbyLow() 7 | { 8 | digitalWrite(STBY, LOW); 9 | } 10 | 11 | void leftForward() 12 | { 13 | digitalWrite(AIN1, HIGH); 14 | digitalWrite(AIN2, LOW); 15 | } 16 | 17 | void leftReverse() 18 | { 19 | digitalWrite(AIN1, LOW); 20 | digitalWrite(AIN2, HIGH); 21 | } 22 | 23 | void leftBrake() 24 | { 25 | digitalWrite(AIN1, HIGH); 26 | digitalWrite(AIN2, HIGH); 27 | } 28 | 29 | void leftForwardBase() 30 | { 31 | stbyHigh(); 32 | leftForward(); 33 | analogWrite(PWMA, leftBase); 34 | } 35 | 36 | void leftReverseBase() 37 | { 38 | stbyHigh(); 39 | leftReverse(); 40 | analogWrite(PWMA, leftBase); 41 | } 42 | 43 | void rightForward() 44 | { 45 | digitalWrite(BIN1, HIGH); 46 | digitalWrite(BIN2, LOW); 47 | } 48 | 49 | void rightReverse() 50 | { 51 | digitalWrite(BIN1, LOW); 52 | digitalWrite(BIN2, HIGH); 53 | } 54 | 55 | void rightBrake() 56 | { 57 | digitalWrite(BIN1, HIGH); 58 | digitalWrite(BIN2, HIGH); 59 | } 60 | 61 | void rightForwardBase() 62 | { 63 | stbyHigh(); 64 | rightForward(); 65 | analogWrite(PWMB, rightBase); 66 | } 67 | 68 | void rightReverseBase() 69 | { 70 | stbyHigh(); 71 | rightReverse(); 72 | analogWrite(PWMB, rightBase); 73 | } 74 | 75 | void writePwm() 76 | { 77 | analogWrite(PWMA, leftPwm); 78 | analogWrite(PWMB, rightPwm); 79 | } 80 | 81 | void writeBasePwm() 82 | { 83 | analogWrite(PWMA, leftBase); 84 | analogWrite(PWMB, rightBase); 85 | } 86 | 87 | void forward() 88 | { 89 | stbyHigh(); 90 | leftForward(); 91 | rightForward(); 92 | writePwm(); 93 | } 94 | 95 | void reverse() 96 | { 97 | stbyHigh(); 98 | leftReverse(); 99 | rightReverse(); 100 | writePwm(); 101 | } 102 | 103 | void brake() 104 | { 105 | stbyHigh(); 106 | leftBrake(); 107 | rightBrake(); 108 | delay(100); 109 | } 110 | 111 | void brakeNo() 112 | { 113 | stbyHigh(); 114 | leftBrake(); 115 | rightBrake(); 116 | //delay(100); 117 | } 118 | 119 | void forwardBase() 120 | { 121 | stbyHigh(); 122 | leftForward(); 123 | rightForward(); 124 | writeBasePwm(); 125 | } 126 | 127 | void reverseBase() 128 | { 129 | stbyHigh(); 130 | leftReverse(); 131 | rightReverse(); 132 | writeBasePwm(); 133 | } 134 | 135 | void turnRight() 136 | { 137 | stbyHigh(); 138 | leftForward(); 139 | rightReverse(); 140 | writeBasePwm(); 141 | } 142 | 143 | void turnLeft() 144 | { 145 | stbyHigh(); 146 | leftReverse(); 147 | rightForward(); 148 | writeBasePwm(); 149 | } 150 | -------------------------------------------------------------------------------- /piccolaMain/piccolaMotors.h: -------------------------------------------------------------------------------- 1 | void stbyHigh() 2 | { 3 | digitalWrite(STBY, HIGH); 4 | } 5 | 6 | void stbyLow() 7 | { 8 | digitalWrite(STBY, LOW); 9 | } 10 | 11 | void leftForward() 12 | { 13 | digitalWrite(AIN1, HIGH); 14 | digitalWrite(AIN2, LOW); 15 | } 16 | 17 | void leftReverse() 18 | { 19 | digitalWrite(AIN1, LOW); 20 | digitalWrite(AIN2, HIGH); 21 | } 22 | 23 | void leftBrake() 24 | { 25 | digitalWrite(AIN1, HIGH); 26 | digitalWrite(AIN2, HIGH); 27 | } 28 | 29 | void leftForwardBase() 30 | { 31 | stbyHigh(); 32 | leftForward(); 33 | analogWrite(PWMA, leftBase); 34 | } 35 | 36 | void leftReverseBase() 37 | { 38 | stbyHigh(); 39 | leftReverse(); 40 | analogWrite(PWMA, leftBase); 41 | } 42 | 43 | void rightForward() 44 | { 45 | digitalWrite(BIN1, HIGH); 46 | digitalWrite(BIN2, LOW); 47 | } 48 | 49 | void rightReverse() 50 | { 51 | digitalWrite(BIN1, LOW); 52 | digitalWrite(BIN2, HIGH); 53 | } 54 | 55 | void rightBrake() 56 | { 57 | digitalWrite(BIN1, HIGH); 58 | digitalWrite(BIN2, HIGH); 59 | } 60 | 61 | void rightForwardBase() 62 | { 63 | stbyHigh(); 64 | rightForward(); 65 | analogWrite(PWMB, rightBase); 66 | } 67 | 68 | void rightReverseBase() 69 | { 70 | stbyHigh(); 71 | rightReverse(); 72 | analogWrite(PWMB, rightBase); 73 | } 74 | 75 | void writePwm() 76 | { 77 | analogWrite(PWMA, leftPwm); 78 | analogWrite(PWMB, rightPwm); 79 | } 80 | 81 | void writeBasePwm() 82 | { 83 | analogWrite(PWMA, leftBase); 84 | analogWrite(PWMB, rightBase); 85 | } 86 | 87 | void forward() 88 | { 89 | stbyHigh(); 90 | leftForward(); 91 | rightForward(); 92 | writePwm(); 93 | } 94 | 95 | void reverse() 96 | { 97 | stbyHigh(); 98 | leftReverse(); 99 | rightReverse(); 100 | writePwm(); 101 | } 102 | 103 | void brake() 104 | { 105 | stbyHigh(); 106 | leftBrake(); 107 | rightBrake(); 108 | delay(75); 109 | } 110 | 111 | void brakeNo() 112 | { 113 | stbyHigh(); 114 | leftBrake(); 115 | rightBrake(); 116 | //delay(100); 117 | } 118 | 119 | void forwardBase() 120 | { 121 | stbyHigh(); 122 | leftForward(); 123 | rightForward(); 124 | writeBasePwm(); 125 | } 126 | 127 | void reverseBase() 128 | { 129 | stbyHigh(); 130 | leftReverse(); 131 | rightReverse(); 132 | writeBasePwm(); 133 | } 134 | 135 | void turnRight() 136 | { 137 | stbyHigh(); 138 | leftForward(); 139 | rightReverse(); 140 | writeBasePwm(); 141 | } 142 | 143 | void turnLeft() 144 | { 145 | stbyHigh(); 146 | leftReverse(); 147 | rightForward(); 148 | writeBasePwm(); 149 | } 150 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaMotors.h: -------------------------------------------------------------------------------- 1 | void stbyHigh() 2 | { 3 | digitalWrite(STBY, HIGH); 4 | } 5 | 6 | void stbyLow() 7 | { 8 | digitalWrite(STBY, LOW); 9 | } 10 | 11 | void leftForward() 12 | { 13 | digitalWrite(AIN1, HIGH); 14 | digitalWrite(AIN2, LOW); 15 | } 16 | 17 | void leftReverse() 18 | { 19 | digitalWrite(AIN1, LOW); 20 | digitalWrite(AIN2, HIGH); 21 | } 22 | 23 | void leftBrake() 24 | { 25 | digitalWrite(AIN1, HIGH); 26 | digitalWrite(AIN2, HIGH); 27 | } 28 | 29 | void leftForwardBase() 30 | { 31 | stbyHigh(); 32 | leftForward(); 33 | analogWrite(PWMA, leftBase); 34 | } 35 | 36 | void leftReverseBase() 37 | { 38 | stbyHigh(); 39 | leftReverse(); 40 | analogWrite(PWMA, leftBase); 41 | } 42 | 43 | void rightForward() 44 | { 45 | digitalWrite(BIN1, HIGH); 46 | digitalWrite(BIN2, LOW); 47 | } 48 | 49 | void rightReverse() 50 | { 51 | digitalWrite(BIN1, LOW); 52 | digitalWrite(BIN2, HIGH); 53 | } 54 | 55 | void rightBrake() 56 | { 57 | digitalWrite(BIN1, HIGH); 58 | digitalWrite(BIN2, HIGH); 59 | } 60 | 61 | void rightForwardBase() 62 | { 63 | stbyHigh(); 64 | rightForward(); 65 | analogWrite(PWMB, rightBase); 66 | } 67 | 68 | void rightReverseBase() 69 | { 70 | stbyHigh(); 71 | rightReverse(); 72 | analogWrite(PWMB, rightBase); 73 | } 74 | 75 | void writePwm() 76 | { 77 | analogWrite(PWMA, leftPwm); 78 | analogWrite(PWMB, rightPwm); 79 | } 80 | 81 | void writeBasePwm() 82 | { 83 | analogWrite(PWMA, leftBase); 84 | analogWrite(PWMB, rightBase); 85 | } 86 | 87 | void forward() 88 | { 89 | stbyHigh(); 90 | leftForward(); 91 | rightForward(); 92 | writePwm(); 93 | } 94 | 95 | void reverse() 96 | { 97 | stbyHigh(); 98 | leftReverse(); 99 | rightReverse(); 100 | writePwm(); 101 | } 102 | 103 | void brake() 104 | { 105 | stbyHigh(); 106 | leftBrake(); 107 | rightBrake(); 108 | delay(1000); 109 | } 110 | 111 | void forwardBase() 112 | { 113 | stbyHigh(); 114 | leftForward(); 115 | rightForward(); 116 | writeBasePwm(); 117 | } 118 | 119 | void reverseBase() 120 | { 121 | stbyHigh(); 122 | leftReverse(); 123 | rightReverse(); 124 | writeBasePwm(); 125 | } 126 | 127 | void turnRight() 128 | { 129 | stbyHigh(); 130 | leftForward(); 131 | rightReverse(); 132 | writeBasePwm(); 133 | } 134 | 135 | void turnLeft() 136 | { 137 | stbyHigh(); 138 | leftReverse(); 139 | rightForward(); 140 | writeBasePwm(); 141 | } 142 | -------------------------------------------------------------------------------- /code/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Win32", 5 | "includePath": [ 6 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\tools**", 7 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28**", 8 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\variants\\generic_stm32f103c", 9 | "${workspaceRoot}", 10 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\**", 11 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\**", 12 | "C:\\Program Files (x86)\\Arduino\\tools\\**", 13 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\**", 14 | "C:\\Program Files (x86)\\Arduino\\libraries\\**", 15 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\lib\\gcc\\avr\\5.4.0\\include", 16 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\VL6180X-1.2.0", 17 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino", 18 | "C:\\Program Files (x86)\\Arduino/hardware\\arduino\\avr\\variants\\standard", 19 | "C:\\Program Files (x86)\\Arduino", 20 | "D:\\micromouse\\piccola\\code", 21 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\**", 22 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\tools\\**" 23 | ], 24 | "forcedInclude": [ 25 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\variants\\generic_stm32f103c\\board.cpp" 26 | ], 27 | "browse": { 28 | "limitSymbolsToIncludedHeaders": false, 29 | "path": [ 30 | "${workspaceRoot}", 31 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino", 32 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\avr\\include", 33 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\lib\\gcc\\avr\\5.4.0\\include", 34 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\VL6180X-1.2.0", 35 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\variants\\standard" 36 | ] 37 | }, 38 | "intelliSenseMode": "msvc-x64", 39 | "cStandard": "c11", 40 | "cppStandard": "c++17", 41 | "defines": [ 42 | "USBCON" 43 | ] 44 | } 45 | ], 46 | "version": 4 47 | } -------------------------------------------------------------------------------- /piccolaMain/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Win32", 5 | "includePath": [ 6 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\tools**", 7 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28**", 8 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\variants\\generic_stm32f103c", 9 | "${workspaceRoot}", 10 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\**", 11 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\**", 12 | "C:\\Program Files (x86)\\Arduino\\tools\\**", 13 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\**", 14 | "C:\\Program Files (x86)\\Arduino\\libraries\\**", 15 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\lib\\gcc\\avr\\5.4.0\\include", 16 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\VL6180X-1.2.0", 17 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino", 18 | "C:\\Program Files (x86)\\Arduino/hardware\\arduino\\avr\\variants\\standard", 19 | "C:\\Program Files (x86)\\Arduino", 20 | "D:\\micromouse\\piccola\\code", 21 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\**", 22 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\tools\\**" 23 | ], 24 | "forcedInclude": [ 25 | "C:\\Users\\ISURU DISSANAYAKE\\AppData\\Local\\Arduino15\\packages\\stm32duino\\hardware\\STM32F1\\2019.7.28\\variants\\generic_stm32f103c\\board.cpp" 26 | ], 27 | "browse": { 28 | "limitSymbolsToIncludedHeaders": false, 29 | "path": [ 30 | "${workspaceRoot}", 31 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino", 32 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\avr\\include", 33 | "C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\lib\\gcc\\avr\\5.4.0\\include", 34 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries\\VL6180X-1.2.0", 35 | "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\variants\\standard" 36 | ] 37 | }, 38 | "intelliSenseMode": "msvc-x64", 39 | "cStandard": "c11", 40 | "cppStandard": "c++17", 41 | "defines": [ 42 | "USBCON" 43 | ] 44 | } 45 | ], 46 | "version": 4 47 | } -------------------------------------------------------------------------------- /API.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | ''' 4 | orients : 5 | 0- North 6 | 1- East 7 | 2- South 8 | 3- West 9 | ''' 10 | 11 | def orientation(orient,turning): 12 | if (turning== 'L'): 13 | orient-=1 14 | if (orient==-1): 15 | orient=3 16 | elif(turning== 'R'): 17 | orient+=1 18 | if (orient==4): 19 | orient=0 20 | elif(turning== 'B'): 21 | if (orient==0): 22 | orient=2 23 | elif (orient==1): 24 | orient=3 25 | elif (orient==2): 26 | orient=0 27 | elif (orient==3): 28 | orient=1 29 | 30 | return(orient) 31 | 32 | def updateCoordinates(x,y,orient): 33 | 34 | if (orient==0): 35 | y+=1 36 | if (orient==1): 37 | x+=1 38 | if (orient==2): 39 | y-=1 40 | if (orient==3): 41 | x-=1 42 | 43 | return(x,y) 44 | 45 | 46 | 47 | class MouseCrashedError(Exception): 48 | pass 49 | 50 | def command(args, return_type=None): 51 | line = " ".join([str(x) for x in args]) + "\n" 52 | sys.stdout.write(line) 53 | sys.stdout.flush() 54 | if return_type: 55 | response = sys.stdin.readline().strip() 56 | if return_type == bool: 57 | return response == "true" 58 | return return_type(response) 59 | 60 | def mazeWidth(): 61 | return command(args=["mazeWidth"], return_type=int) 62 | 63 | def mazeHeight(): 64 | return command(args=["mazeHeight"], return_type=int) 65 | 66 | def wallFront(): 67 | return command(args=["wallFront"], return_type=bool) 68 | 69 | def wallRight(): 70 | return command(args=["wallRight"], return_type=bool) 71 | 72 | def wallLeft(): 73 | return command(args=["wallLeft"], return_type=bool) 74 | 75 | def moveForward(): 76 | response = command(args=["moveForward"], return_type=str) 77 | 78 | if response == "crash": 79 | #log(str(cells[y][x])) 80 | raise MouseCrashedError() 81 | 82 | def turnRight(): 83 | command(args=["turnRight"], return_type=str) 84 | 85 | def turnLeft(): 86 | command(args=["turnLeft"], return_type=str) 87 | 88 | def setWall(x, y, direction): 89 | command(args=["setWall", x, y, direction]) 90 | 91 | def clearWall(x, y, direction): 92 | command(args=["clearWall", x, y, direction]) 93 | 94 | def setColor(x, y, color): 95 | command(args=["setColor", x, y, color]) 96 | 97 | def clearColor(x, y): 98 | command(args=["clearColor", x, y]) 99 | 100 | def clearAllColor(): 101 | command(args=["clearAllColor"]) 102 | 103 | def setText(x, y, text): 104 | command(args=["setText", x, y, text]) 105 | 106 | def clearText(x, y): 107 | command(args=["clearText", x, y]) 108 | 109 | def clearAllText(): 110 | command(args=["clearAllText"]) 111 | 112 | def wasReset(): 113 | return command(args=["wasReset"], return_type=bool) 114 | 115 | def ackReset(): 116 | command(args=["ackReset"], return_type=str) 117 | 118 | def log(string): 119 | sys.stderr.write("{}\n".format(string)) -------------------------------------------------------------------------------- /code/EEPROM.h: -------------------------------------------------------------------------------- 1 | #ifndef __EEPROM_H 2 | #define __EEPROM_H 3 | 4 | #include 5 | #include "flash_stm32.h" 6 | 7 | // HACK ALERT. This definition may not match your processor 8 | // To Do. Work out correct value for EEPROM_PAGE_SIZE on the STM32F103CT6 etc 9 | #define MCU_STM32F103RB 10 | 11 | #ifndef EEPROM_PAGE_SIZE 12 | #if defined (MCU_STM32F103RB) 13 | #define EEPROM_PAGE_SIZE (uint16)0x400 /* Page size = 1KByte */ 14 | #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE) || defined (MCU_STM32F103RD) 15 | #define EEPROM_PAGE_SIZE (uint16)0x800 /* Page size = 2KByte */ 16 | #else 17 | #error "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)." 18 | #endif 19 | #endif 20 | 21 | #ifndef EEPROM_START_ADDRESS 22 | #if defined (MCU_STM32F103RB) 23 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 128 * 1024 - 2 * EEPROM_PAGE_SIZE)) 24 | #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE) 25 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 512 * 1024 - 2 * EEPROM_PAGE_SIZE)) 26 | #elif defined (MCU_STM32F103RD) 27 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 384 * 1024 - 2 * EEPROM_PAGE_SIZE)) 28 | #else 29 | #error "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)." 30 | #endif 31 | #endif 32 | 33 | /* Pages 0 and 1 base and end addresses */ 34 | #define EEPROM_PAGE0_BASE ((uint32)(EEPROM_START_ADDRESS + 0x000)) 35 | #define EEPROM_PAGE1_BASE ((uint32)(EEPROM_START_ADDRESS + EEPROM_PAGE_SIZE)) 36 | 37 | /* Page status definitions */ 38 | #define EEPROM_ERASED ((uint16)0xFFFF) /* PAGE is empty */ 39 | #define EEPROM_RECEIVE_DATA ((uint16)0xEEEE) /* PAGE is marked to receive data */ 40 | #define EEPROM_VALID_PAGE ((uint16)0x0000) /* PAGE containing valid data */ 41 | 42 | /* Page full define */ 43 | enum : uint16 44 | { 45 | EEPROM_OK = ((uint16)0x0000), 46 | EEPROM_OUT_SIZE = ((uint16)0x0081), 47 | EEPROM_BAD_ADDRESS = ((uint16)0x0082), 48 | EEPROM_BAD_FLASH = ((uint16)0x0083), 49 | EEPROM_NOT_INIT = ((uint16)0x0084), 50 | EEPROM_SAME_VALUE = ((uint16)0x0085), 51 | EEPROM_NO_VALID_PAGE = ((uint16)0x00AB) 52 | }; 53 | 54 | #define EEPROM_DEFAULT_DATA 0xFFFF 55 | 56 | 57 | class EEPROMClass 58 | { 59 | public: 60 | EEPROMClass(void); 61 | 62 | uint16 init(void); 63 | uint16 init(uint32, uint32, uint32); 64 | 65 | uint16 format(void); 66 | 67 | uint16 erases(uint16 *); 68 | uint16 read (uint16 address); 69 | uint16 read (uint16 address, uint16 *data); 70 | uint16 write(uint16 address, uint16 data); 71 | uint16 update(uint16 address, uint16 data); 72 | uint16 count(uint16 *); 73 | uint16 maxcount(void); 74 | 75 | uint32 PageBase0; 76 | uint32 PageBase1; 77 | uint32 PageSize; 78 | uint16 Status; 79 | private: 80 | FLASH_Status EE_ErasePage(uint32); 81 | 82 | uint16 EE_CheckPage(uint32, uint16); 83 | uint16 EE_CheckErasePage(uint32, uint16); 84 | uint16 EE_Format(void); 85 | uint32 EE_FindValidPage(void); 86 | uint16 EE_GetVariablesCount(uint32, uint16); 87 | uint16 EE_PageTransfer(uint32, uint32, uint16); 88 | uint16 EE_VerifyPageFullWriteVariable(uint16, uint16); 89 | }; 90 | 91 | extern EEPROMClass EEPROM; 92 | 93 | #endif /* __EEPROM_H */ 94 | -------------------------------------------------------------------------------- /piccolaMain/EEPROM.h: -------------------------------------------------------------------------------- 1 | #ifndef __EEPROM_H 2 | #define __EEPROM_H 3 | 4 | #include 5 | #include "flash_stm32.h" 6 | 7 | // HACK ALERT. This definition may not match your processor 8 | // To Do. Work out correct value for EEPROM_PAGE_SIZE on the STM32F103CT6 etc 9 | #define MCU_STM32F103RB 10 | 11 | #ifndef EEPROM_PAGE_SIZE 12 | #if defined (MCU_STM32F103RB) 13 | #define EEPROM_PAGE_SIZE (uint16)0x400 /* Page size = 1KByte */ 14 | #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE) || defined (MCU_STM32F103RD) 15 | #define EEPROM_PAGE_SIZE (uint16)0x800 /* Page size = 2KByte */ 16 | #else 17 | #error "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)." 18 | #endif 19 | #endif 20 | 21 | #ifndef EEPROM_START_ADDRESS 22 | #if defined (MCU_STM32F103RB) 23 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 128 * 1024 - 2 * EEPROM_PAGE_SIZE)) 24 | #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE) 25 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 512 * 1024 - 2 * EEPROM_PAGE_SIZE)) 26 | #elif defined (MCU_STM32F103RD) 27 | #define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 384 * 1024 - 2 * EEPROM_PAGE_SIZE)) 28 | #else 29 | #error "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)." 30 | #endif 31 | #endif 32 | 33 | /* Pages 0 and 1 base and end addresses */ 34 | #define EEPROM_PAGE0_BASE ((uint32)(EEPROM_START_ADDRESS + 0x000)) 35 | #define EEPROM_PAGE1_BASE ((uint32)(EEPROM_START_ADDRESS + EEPROM_PAGE_SIZE)) 36 | 37 | /* Page status definitions */ 38 | #define EEPROM_ERASED ((uint16)0xFFFF) /* PAGE is empty */ 39 | #define EEPROM_RECEIVE_DATA ((uint16)0xEEEE) /* PAGE is marked to receive data */ 40 | #define EEPROM_VALID_PAGE ((uint16)0x0000) /* PAGE containing valid data */ 41 | 42 | /* Page full define */ 43 | enum : uint16 44 | { 45 | EEPROM_OK = ((uint16)0x0000), 46 | EEPROM_OUT_SIZE = ((uint16)0x0081), 47 | EEPROM_BAD_ADDRESS = ((uint16)0x0082), 48 | EEPROM_BAD_FLASH = ((uint16)0x0083), 49 | EEPROM_NOT_INIT = ((uint16)0x0084), 50 | EEPROM_SAME_VALUE = ((uint16)0x0085), 51 | EEPROM_NO_VALID_PAGE = ((uint16)0x00AB) 52 | }; 53 | 54 | #define EEPROM_DEFAULT_DATA 0xFFFF 55 | 56 | 57 | class EEPROMClass 58 | { 59 | public: 60 | EEPROMClass(void); 61 | 62 | uint16 init(void); 63 | uint16 init(uint32, uint32, uint32); 64 | 65 | uint16 format(void); 66 | 67 | uint16 erases(uint16 *); 68 | uint16 read (uint16 address); 69 | uint16 read (uint16 address, uint16 *data); 70 | uint16 write(uint16 address, uint16 data); 71 | uint16 update(uint16 address, uint16 data); 72 | uint16 count(uint16 *); 73 | uint16 maxcount(void); 74 | 75 | uint32 PageBase0; 76 | uint32 PageBase1; 77 | uint32 PageSize; 78 | uint16 Status; 79 | private: 80 | FLASH_Status EE_ErasePage(uint32); 81 | 82 | uint16 EE_CheckPage(uint32, uint16); 83 | uint16 EE_CheckErasePage(uint32, uint16); 84 | uint16 EE_Format(void); 85 | uint32 EE_FindValidPage(void); 86 | uint16 EE_GetVariablesCount(uint32, uint16); 87 | uint16 EE_PageTransfer(uint32, uint32, uint16); 88 | uint16 EE_VerifyPageFullWriteVariable(uint16, uint16); 89 | }; 90 | 91 | extern EEPROMClass EEPROM; 92 | 93 | #endif /* __EEPROM_H */ 94 | -------------------------------------------------------------------------------- /code/piccolaSpeed.h: -------------------------------------------------------------------------------- 1 | void cellStart() 2 | { 3 | leftWallAvailable = 0; 4 | rightWallAvailable = 0; 5 | frontWallAvailable = 0; 6 | encoderRightCount = encoderRightCount + 220; 7 | encoderLeftCount = encoderLeftCount + 220; 8 | rightBase=70; 9 | leftBase=70; 10 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 11 | { 12 | dif = leftEncoder - encoderLeftCount + 220; 13 | rightBase = 70 + int(dif/2); 14 | leftBase = 70 + int(dif/2); 15 | forwardBase(); 16 | } 17 | rightBase=176; 18 | leftBase=180; 19 | encoderRightCount= encoderRightCount + 267; 20 | encoderLeftCount= encoderLeftCount + 267; 21 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 22 | { 23 | wallFollow(); 24 | } 25 | encoderLeftCount = encoderLeftCount + 150; 26 | encoderRightCount = encoderRightCount + 150; 27 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 28 | { 29 | wallFollow(); 30 | //encoderPid(); 31 | if (tof[2] <= 180) 32 | { 33 | frontWallAvailable = frontWallAvailable + 1; 34 | } 35 | else 36 | { 37 | frontWallAvailable = frontWallAvailable - 1; 38 | } 39 | if (tof[0] <= 150) 40 | { 41 | leftWallAvailable= leftWallAvailable + 1; 42 | } 43 | else 44 | { 45 | leftWallAvailable= leftWallAvailable - 1; 46 | } 47 | if (tof[4] <= 150) 48 | { 49 | rightWallAvailable= rightWallAvailable + 1; 50 | } 51 | else 52 | { 53 | rightWallAvailable= rightWallAvailable - 1; 54 | } 55 | } 56 | } 57 | 58 | void cellBrake() 59 | { 60 | encoderRightCount= encoderRightCount + 417; 61 | encoderLeftCount= encoderLeftCount + 417; 62 | rightBase=176; 63 | leftBase=180; 64 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 65 | { 66 | wallFollow(); 67 | } 68 | encoderRightCount= encoderRightCount + 220; 69 | encoderLeftCount= encoderLeftCount + 220; 70 | if (F == true) 71 | { 72 | tof[2] = tof3.readRangeSingleMillimeters(); 73 | while (tof[2] > 52) 74 | { 75 | tof[2] = tof3.readRangeSingleMillimeters(); 76 | dif = leftEncoder - encoderLeftCount + 220; 77 | rightBase = 176 - int(dif*0.75); 78 | leftBase = 180 - int(dif*0.85); 79 | if (rightBase <= 70) 80 | { 81 | rightBase = 70; 82 | } 83 | if (leftBase <= 70) 84 | { 85 | leftBase = 70; 86 | } 87 | forwardBase(); 88 | } 89 | } 90 | else 91 | { 92 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 93 | { 94 | dif = leftEncoder - encoderLeftCount + 220; 95 | rightBase = 176 - int(dif/2); 96 | leftBase = 180 - int((dif*3)/5); 97 | if (rightBase <= 70) 98 | { 99 | rightBase = 70; 100 | } 101 | if (leftBase <= 70) 102 | { 103 | leftBase = 70; 104 | } 105 | forwardBase(); 106 | } 107 | } 108 | brake(); 109 | } 110 | -------------------------------------------------------------------------------- /arduinoAlgoCheck/piccolaZlgoVariables.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | byte x=0; 4 | byte y=0; 5 | 6 | byte xprev=0; 7 | byte yprev=0; 8 | byte orient=0; 9 | byte state=0; 10 | boolean shortPath= false; 11 | boolean middleSquare= true; 12 | byte x_0; 13 | byte y_0; 14 | byte x_1; 15 | byte y_1; 16 | byte x_2; 17 | byte y_2; 18 | byte x_3; 19 | byte y_3; 20 | byte mazeSize = 16; 21 | char dir; 22 | char turning; 23 | QueueArray queue; 24 | QueueArray pathQueue; 25 | boolean L = true; 26 | boolean R= true; 27 | boolean F= false; 28 | byte xdes; 29 | byte ydes; 30 | unsigned long timer; 31 | int cellCount =1; 32 | 33 | 34 | byte cells[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 35 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 36 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 37 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 38 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 39 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 40 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 41 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 42 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 43 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 44 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 45 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 46 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 47 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 48 | 49 | byte flood2[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 50 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 51 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 52 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 53 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 54 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 55 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 56 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 57 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 58 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 59 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 60 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 61 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 62 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 63 | 64 | byte flood[14][14]={{12,11,10,9,8,7,6,6,7,8,9,10,11,12}, 65 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 66 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 67 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 68 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 69 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 70 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 71 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 72 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 73 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 74 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 75 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 76 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 77 | {12,11,10,9,8,7,6,6,7,8,9,10,11,12}}; 78 | 79 | byte sliit[14][14] ={{11,11,11,11,14,10,12,5,6,5,4,10,10,12}, 80 | {9,9,9,8,10,4,10,3,9,9,8,10,12,11}, 81 | {1,3,8,10,4,7,5,3,13,9,14,4,10,3}, 82 | {9,8,10,10,15,10,7,1,10,7,11,9,5,3}, 83 | {1,4,4,4,7,5,10,7,5,4,2,2,7,9}, 84 | {9,13,9,1,10,3,5,10,3,9,5,4,4,9}, 85 | {1,4,7,9,11,9,1,6,9,1,7,9,9,9}, 86 | {9,9,14,7,9,9,8,7,9,1,10,7,9,9}, 87 | {13,8,10,10,7,8,10,10,3,8,10,10,3,9}, 88 | {11,11,11,14,6,11,11,5,2,4,12,5,2,7}, 89 | {9,9,8,6,9,1,15,3,14,15,4,15,10,6}, 90 | {9,1,10,3,9,9,13,1,12,9,13,8,6,9}, 91 | {9,8,12,1,3,1,4,15,10,3,5,10,7,9}, 92 | {8,10,10,7,8,7,13,13,11,13,13,5,10,7}}; 93 | 94 | char bluetoothWalls; 95 | byte xrun=0; 96 | byte yrun=0; 97 | 98 | -------------------------------------------------------------------------------- /code/piccolaZlgoVariables.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | byte x=0; 4 | byte y=0; 5 | 6 | byte xprev=0; 7 | byte yprev=0; 8 | byte orient=0; 9 | byte state=0; 10 | boolean shortPath= false; 11 | boolean middleSquare= true; 12 | byte x_0; 13 | byte y_0; 14 | byte x_1; 15 | byte y_1; 16 | byte x_2; 17 | byte y_2; 18 | byte x_3; 19 | byte y_3; 20 | byte mazeSize = 16; 21 | char dir; 22 | char turning; 23 | QueueArray queue; 24 | QueueArray pathQueue; 25 | boolean L = true; 26 | boolean R= true; 27 | boolean F= false; 28 | byte xdes; 29 | byte ydes; 30 | unsigned long timer; 31 | int cellCount =0; 32 | boolean runningNext; 33 | 34 | byte cells[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 35 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 36 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 37 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 38 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 39 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 40 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 41 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 42 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 43 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 44 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 45 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 46 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 47 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 48 | 49 | byte flood2[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 50 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 51 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 52 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 53 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 54 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 55 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 56 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 57 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 58 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 59 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 60 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 61 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 62 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 63 | 64 | byte flood[14][14]={{12,11,10,9,8,7,6,6,7,8,9,10,11,12}, 65 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 66 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 67 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 68 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 69 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 70 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 71 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 72 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 73 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 74 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 75 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 76 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 77 | {12,11,10,9,8,7,6,6,7,8,9,10,11,12}}; 78 | 79 | byte sliit[14][14] ={{11,11,11,11,14,10,12,5,6,5,4,10,10,12}, 80 | {9,9,9,8,10,4,10,3,9,9,8,10,12,11}, 81 | {1,3,8,10,4,7,5,3,13,9,14,4,10,3}, 82 | {9,8,10,10,15,10,7,1,10,7,11,9,5,3}, 83 | {1,4,4,4,7,5,10,7,5,4,2,2,7,9}, 84 | {9,13,9,1,10,3,5,10,3,9,5,4,4,9}, 85 | {1,4,7,9,11,9,1,6,9,1,7,9,9,9}, 86 | {9,9,14,7,9,9,8,7,9,1,10,7,9,9}, 87 | {13,8,10,10,7,8,10,10,3,8,10,10,3,9}, 88 | {11,11,11,14,6,11,11,5,2,4,12,5,2,7}, 89 | {9,9,8,6,9,1,15,3,14,15,4,15,10,6}, 90 | {9,1,10,3,9,9,13,1,12,9,13,8,6,9}, 91 | {9,8,12,1,3,1,4,15,10,3,5,10,7,9}, 92 | {8,10,10,7,8,7,13,13,11,13,13,5,10,7}}; 93 | 94 | char bluetoothWalls; 95 | byte xrun=0; 96 | byte yrun=0; 97 | 98 | -------------------------------------------------------------------------------- /piccolaMain/piccolaZlgoVariables.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | byte x=0; 4 | byte y=0; 5 | 6 | byte xprev=0; 7 | byte yprev=0; 8 | byte orient=0; 9 | byte state=0; 10 | boolean shortPath= false; 11 | boolean middleSquare= true; 12 | byte x_0; 13 | byte y_0; 14 | byte x_1; 15 | byte y_1; 16 | byte x_2; 17 | byte y_2; 18 | byte x_3; 19 | byte y_3; 20 | byte mazeSize = 16; 21 | char dir; 22 | char turning; 23 | QueueArray queue; 24 | QueueArray pathQueue; 25 | boolean L = true; 26 | boolean R= true; 27 | boolean F= false; 28 | byte xdes; 29 | byte ydes; 30 | unsigned long timer; 31 | int cellCount =0; 32 | boolean runningNext; 33 | 34 | byte cells[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 35 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 36 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 37 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 38 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 39 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 40 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 41 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 42 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 43 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 44 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 45 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 46 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 47 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 48 | 49 | byte flood2[14][14] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 50 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 51 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 52 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 53 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 54 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 55 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 56 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 57 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 58 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 59 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 60 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 61 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 62 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0}}; 63 | 64 | byte flood[14][14]={{12,11,10,9,8,7,6,6,7,8,9,10,11,12}, 65 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 66 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 67 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 68 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 69 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 70 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 71 | {6,5,4,3,2,1,0,0,1,2,3,4,5,6}, 72 | {7,6,5,4,3,2,1,1,2,3,4,5,6,7}, 73 | {8,7,6,5,4,3,2,2,3,4,5,6,7,8}, 74 | {9,8,7,6,5,4,3,3,4,5,6,7,8,9}, 75 | {10,9,8,7,6,5,4,4,5,6,7,8,9,10}, 76 | {11,10,9,8,7,6,5,5,6,7,8,9,10,11}, 77 | {12,11,10,9,8,7,6,6,7,8,9,10,11,12}}; 78 | 79 | byte sliit[14][14] ={{11,11,11,11,14,10,12,5,6,5,4,10,10,12}, 80 | {9,9,9,8,10,4,10,3,9,9,8,10,12,11}, 81 | {1,3,8,10,4,7,5,3,13,9,14,4,10,3}, 82 | {9,8,10,10,15,10,7,1,10,7,11,9,5,3}, 83 | {1,4,4,4,7,5,10,7,5,4,2,2,7,9}, 84 | {9,13,9,1,10,3,5,10,3,9,5,4,4,9}, 85 | {1,4,7,9,11,9,1,6,9,1,7,9,9,9}, 86 | {9,9,14,7,9,9,8,7,9,1,10,7,9,9}, 87 | {13,8,10,10,7,8,10,10,3,8,10,10,3,9}, 88 | {11,11,11,14,6,11,11,5,2,4,12,5,2,7}, 89 | {9,9,8,6,9,1,15,3,14,15,4,15,10,6}, 90 | {9,1,10,3,9,9,13,1,12,9,13,8,6,9}, 91 | {9,8,12,1,3,1,4,15,10,3,5,10,7,9}, 92 | {8,10,10,7,8,7,13,13,11,13,13,5,10,7}}; 93 | 94 | char bluetoothWalls; 95 | byte xrun=0; 96 | byte yrun=0; 97 | 98 | -------------------------------------------------------------------------------- /piccolaMain/piccolaSpeed.h: -------------------------------------------------------------------------------- 1 | void cellStart() 2 | { 3 | leftWallAvailable = 0; 4 | rightWallAvailable = 0; 5 | frontWallAvailable = 0; 6 | encoderRightCount = encoderRightCount + 220; 7 | encoderLeftCount = encoderLeftCount + 220; 8 | rightBase=70; 9 | leftBase=70; 10 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 11 | { 12 | dif = leftEncoder - encoderLeftCount + 220; 13 | rightBase = 70 + int(dif/2); 14 | leftBase = 70 + int(dif/2); 15 | forwardBase(); 16 | } 17 | rightBase=176; 18 | leftBase=180; 19 | encoderRightCount= encoderRightCount + 267; 20 | encoderLeftCount= encoderLeftCount + 267; 21 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 22 | { 23 | wallFollow(); 24 | } 25 | encoderLeftCount = encoderLeftCount + 150; 26 | encoderRightCount = encoderRightCount + 150; 27 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 28 | { 29 | wallFollow(); 30 | //encoderPid(); 31 | if (tof[2] <= 180) 32 | { 33 | frontWallAvailable = frontWallAvailable + 1; 34 | } 35 | else 36 | { 37 | frontWallAvailable = frontWallAvailable - 1; 38 | } 39 | if (tof[0] <= 160) 40 | { 41 | leftWallAvailable= leftWallAvailable + 1; 42 | } 43 | else 44 | { 45 | leftWallAvailable= leftWallAvailable - 1; 46 | } 47 | if (tof[4] <= 160) 48 | { 49 | rightWallAvailable= rightWallAvailable + 1; 50 | } 51 | else 52 | { 53 | rightWallAvailable= rightWallAvailable - 1; 54 | } 55 | } 56 | } 57 | 58 | void cellBrake() 59 | { 60 | encoderRightCount= encoderRightCount + 417; 61 | encoderLeftCount= encoderLeftCount + 417; 62 | rightBase=176; 63 | leftBase=180; 64 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 65 | { 66 | wallFollow(); 67 | } 68 | encoderRightCount= encoderRightCount + 220; 69 | encoderLeftCount= encoderLeftCount + 220; 70 | if (F == true) 71 | { 72 | tof[2] = tof3.readRangeSingleMillimeters(); 73 | while (tof[2] > 52) 74 | { 75 | tof[2] = tof3.readRangeSingleMillimeters(); 76 | dif = leftEncoder - encoderLeftCount + 220; 77 | rightBase = 176 - int(dif*0.75); 78 | leftBase = 180 - int(dif*0.85); 79 | if (rightBase <= 70) 80 | { 81 | rightBase = 70; 82 | } 83 | if (leftBase <= 70) 84 | { 85 | leftBase = 70; 86 | } 87 | forwardBase(); 88 | } 89 | 90 | leftBase = 70; 91 | rightBase = 70; 92 | tof[2] = tof3.readRangeSingleMillimeters(); 93 | while (tof[2] < 50) 94 | { 95 | reverseBase(); 96 | tof[2] = tof3.readRangeSingleMillimeters(); 97 | } 98 | } 99 | else 100 | { 101 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 102 | { 103 | dif = leftEncoder - encoderLeftCount + 220; 104 | rightBase = 176 - int(dif/2); 105 | leftBase = 180 - int((dif*3)/5); 106 | if (rightBase <= 70) 107 | { 108 | rightBase = 70; 109 | } 110 | if (leftBase <= 70) 111 | { 112 | leftBase = 70; 113 | } 114 | forwardBase(); 115 | } 116 | } 117 | brake(); 118 | } 119 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaTurns.h: -------------------------------------------------------------------------------- 1 | void countLeftOut1() 2 | { 3 | if (digitalRead(leftOut1) == HIGH && digitalRead(leftOut2) == LOW) 4 | { 5 | leftEncoder = leftEncoder + 1; 6 | } 7 | else if (digitalRead(leftOut1) == HIGH && digitalRead(leftOut2) == HIGH) 8 | { 9 | leftEncoder = leftEncoder - 1; 10 | } 11 | } 12 | 13 | void countLeftOut2() 14 | { 15 | if (digitalRead(leftOut2) == HIGH && digitalRead(leftOut1) == HIGH) 16 | { 17 | leftEncoder = leftEncoder + 1; 18 | } 19 | else if (digitalRead(leftOut2) == HIGH && digitalRead(leftOut1) == LOW) 20 | { 21 | leftEncoder = leftEncoder - 1; 22 | } 23 | } 24 | 25 | void countRightOut1() 26 | { 27 | if (digitalRead(rightOut1) == HIGH && digitalRead(rightOut2) == LOW) 28 | { 29 | rightEncoder = rightEncoder - 1; 30 | } 31 | else if (digitalRead(rightOut1) == HIGH && digitalRead(rightOut2) == HIGH) 32 | { 33 | rightEncoder = rightEncoder + 1; 34 | } 35 | } 36 | 37 | void countRightOut2() 38 | { 39 | if (digitalRead(rightOut2) == HIGH && digitalRead(rightOut1) == HIGH) 40 | { 41 | rightEncoder = rightEncoder - 1; 42 | } 43 | else if (digitalRead(rightOut2) == HIGH && digitalRead(rightOut1) == LOW) 44 | { 45 | rightEncoder = rightEncoder + 1; 46 | } 47 | } 48 | 49 | void turnRightEncoder() 50 | { 51 | leftEncoder = 0; 52 | rightEncoder = 0; 53 | while (leftEncoder <= encoderLeftCount || rightEncoder >= encoderRightCount) 54 | { 55 | if (rightEncoder >= encoderRightCount) 56 | { 57 | rightReverseBase(); 58 | } 59 | else 60 | { 61 | rightBrake(); 62 | } 63 | 64 | if (leftEncoder <= encoderLeftCount) 65 | { 66 | leftForwardBase(); 67 | } 68 | else 69 | { 70 | leftBrake(); 71 | } 72 | } 73 | brake(); 74 | encoderLeftCount = 0; 75 | encoderRightCount = 0; 76 | leftEncoder = 0; 77 | rightEncoder = 0; 78 | } 79 | 80 | void turnLeftEncoder() 81 | { 82 | leftEncoder = 0; 83 | rightEncoder = 0; 84 | while (rightEncoder <= encoderRightCount || leftEncoder >= encoderLeftCount) 85 | { 86 | if (rightEncoder <= encoderRightCount) 87 | { 88 | rightForwardBase(); 89 | } 90 | else 91 | { 92 | rightBrake(); 93 | } 94 | 95 | if (leftEncoder >= encoderLeftCount) 96 | { 97 | leftReverseBase(); 98 | } 99 | else 100 | { 101 | leftBrake(); 102 | } 103 | 104 | } 105 | brake(); 106 | encoderLeftCount = 0; 107 | encoderRightCount = 0; 108 | leftEncoder = 0; 109 | rightEncoder = 0; 110 | } 111 | 112 | void turnNinetyLeft() 113 | { 114 | encoderRightCount = 440; 115 | encoderLeftCount = -440; 116 | turnLeftEncoder(); 117 | } 118 | 119 | void turnNinetyRight() 120 | { 121 | encoderLeftCount = 440; 122 | encoderRightCount = -440; 123 | turnRightEncoder(); 124 | } 125 | 126 | void rightTurn() 127 | { 128 | leftEncoder = 0; 129 | rightEncoder = 0; 130 | encoderLeftCount = 1500; 131 | leftPwm = leftBase + 75; 132 | rightPwm = rightBase - 80; 133 | while (leftEncoder <= encoderLeftCount) 134 | { 135 | forward(); 136 | } 137 | brake(); 138 | leftEncoder = 0; 139 | rightEncoder = 0; 140 | encoderLeftCount = 0; 141 | encoderRightCount = 0; 142 | } 143 | 144 | void leftTurn() 145 | { 146 | leftEncoder = 0; 147 | rightEncoder = 0; 148 | encoderRightCount = 1500; 149 | leftPwm = leftBase - 85; 150 | rightPwm = rightBase + 75; 151 | while (rightEncoder <= encoderRightCount) 152 | { 153 | forward(); 154 | } 155 | brake(); 156 | leftEncoder = 0; 157 | rightEncoder = 0; 158 | encoderLeftCount = 0; 159 | encoderRightCount = 0; 160 | } -------------------------------------------------------------------------------- /arduinoAlgoCheck/piccolaMazeSolve.h: -------------------------------------------------------------------------------- 1 | 2 | void calculatePath(boolean runningNext,boolean eeprom){ 3 | floodFill2(); 4 | showFlood2(); 5 | 6 | x=0; 7 | y=0; 8 | byte oldOrient= orient; 9 | 10 | cellCount=0; 11 | 12 | while(flood2[y][x]!=1){ 13 | 14 | toMove2(); 15 | 16 | //Serial.println(20); 17 | //Serial.print(x); Serial.print(' '); Serial.println(y); 18 | //Serial.println(dir); 19 | 20 | if (runningNext){ 21 | pathQueue.enqueue(dir); 22 | 23 | } 24 | if(eeprom){ 25 | cellCount++; 26 | EEPROM.write(cellCount,dir);} 27 | 28 | 29 | if (dir=='L'){ 30 | orient = orientation(orient,'L'); 31 | } 32 | 33 | else if (dir=='R'){ 34 | orient = orientation(orient,'R'); 35 | } 36 | 37 | else if (dir=='B'){ 38 | orient = orientation(orient,'L'); 39 | orient = orientation(orient,'L'); 40 | } 41 | 42 | 43 | xprev=x; 44 | yprev=y; 45 | updateCoordinates(); 46 | //prevDir= dir; 47 | 48 | } 49 | 50 | 51 | x=0; 52 | y=0; 53 | orient=oldOrient; 54 | Serial.print(x); Serial.print(' '); Serial.println(y); 55 | Serial.println(orient); 56 | 57 | if(eeprom){ 58 | EEPROM.write(0,1); 59 | EEPROM.write(1,cellCount-1);} 60 | } 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | void traverse(byte xdes, byte ydes, boolean middleSquare, boolean shortPath){ 70 | int forwardCellCount=0; 71 | 72 | if (shortPath== false){ 73 | appendDestination(xdes,ydes,middleSquare); 74 | floodFill3(); 75 | 76 | //tofPid(); 77 | //tofPid(); 78 | //checkWallsCell(); 79 | //updateWalls(x, y, orient, L, R, F); 80 | 81 | while(flood[y][x]!=0){ 82 | 83 | cells[y][x]= sliit[y][x]; 84 | 85 | appendDestination(xdes,ydes,middleSquare); 86 | floodFill3(); 87 | dir= toMove(x,y,xprev,yprev,orient); 88 | 89 | Serial.println(dir); 90 | 91 | if (dir=='L'){ 92 | orient = orientation(orient,'L'); 93 | //leftTurn(); 94 | } 95 | 96 | else if (dir=='R'){ 97 | orient = orientation(orient,'R'); 98 | //rightTurn(); 99 | } 100 | 101 | else if (dir=='B'){ 102 | orient = orientation(orient,'L'); 103 | orient = orientation(orient,'L'); 104 | //cellBack(); 105 | } 106 | else{ 107 | if(x==0 && y==0){ 108 | //cellStart(); 109 | //brake(); 110 | //delay(1000); 111 | } 112 | else{ 113 | //cellForward(); 114 | } 115 | } 116 | 117 | 118 | xprev=x; 119 | yprev=y; 120 | updateCoordinates(); 121 | 122 | 123 | } 124 | } 125 | 126 | else{ 127 | //calculatePath (runningNext, eeprom) 128 | calculatePath(true,false); 129 | showFlood2(); 130 | 131 | while (!pathQueue.isEmpty ()){ 132 | 133 | dir= pathQueue.dequeue(); 134 | 135 | if (dir=='L'){ 136 | orient = orientation(orient,'L'); 137 | //leftSmoothTurn(); 138 | xprev=x; 139 | yprev=y; 140 | updateCoordinates(); 141 | } 142 | 143 | else if (dir=='R'){ 144 | orient = orientation(orient,'R'); 145 | //rightSmoothTurn(); 146 | xprev=x; 147 | yprev=y; 148 | updateCoordinates(); 149 | } 150 | 151 | else if (dir=='B'){ 152 | orient = orientation(orient,'L'); 153 | orient = orientation(orient,'L'); 154 | //cellBack(); 155 | xprev=x; 156 | yprev=y; 157 | updateCoordinates(); 158 | } 159 | else{ 160 | forwardCellCount= String(dir).toInt(); 161 | ///////////////////////you have to move this much of cells forward. accelarate as you wish 162 | 163 | // but call xprev=x; yprev=y; updateCoordinates(); every time you run a cell. 164 | for (int i= forwardCellCount; i>0; i--){ 165 | //cellForward(); 166 | xprev=x; 167 | yprev=y; 168 | updateCoordinates(); 169 | } 170 | 171 | } 172 | Serial.print(x); Serial.print(' '); Serial.println(y); 173 | Serial.println(dir); 174 | 175 | 176 | 177 | 178 | } 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /code/piccolaWallPid.h: -------------------------------------------------------------------------------- 1 | 2 | void leftPid() 3 | { 4 | leftError = 85 - tof[0]; 5 | leftDiff = leftError - leftLastError; 6 | 7 | if (leftDiff > 50 ) 8 | { 9 | leftError = 10; 10 | leftDiff = 5; 11 | } 12 | 13 | else if (leftDiff < -50) 14 | { 15 | leftError = -10; 16 | leftDiff = -5; 17 | } 18 | 19 | correction = (leftError * leftP) + (leftDiff * leftD); 20 | leftLastError = leftError; 21 | 22 | if (correction > 50 ) 23 | { 24 | correction = 15; 25 | } 26 | 27 | else if (correction < -50) 28 | { 29 | correction = -15; 30 | } 31 | 32 | leftPwm = leftBase + correction; 33 | rightPwm = rightBase - correction; 34 | } 35 | 36 | void rightPid() 37 | { 38 | rightError = 80 - tof[4]; 39 | rightDiff = rightError - rightLastError; 40 | if (rightDiff > 50 ) 41 | { 42 | rightError = 10; 43 | rightDiff = 5; 44 | } 45 | 46 | else if (rightDiff < -50) 47 | { 48 | rightError = -10; 49 | rightDiff = -5; 50 | } 51 | 52 | correction = (rightError * rightP) + (rightDiff * rightD); 53 | rightLastError = rightError; 54 | 55 | if (correction > 50 ) 56 | { 57 | correction = 15; 58 | } 59 | 60 | else if (correction < -50) 61 | { 62 | correction = -15; 63 | } 64 | 65 | leftPwm = leftBase - correction; 66 | rightPwm = rightBase + correction; 67 | 68 | } 69 | 70 | 71 | void wallPid() 72 | { 73 | wallError = tof[0] - (tof[4]-8); 74 | rightLastError = 58 - tof[4]; 75 | leftLastError = 50 - tof[0]; 76 | wallDiff = wallError - wallLastError; 77 | 78 | if (wallDiff > 50 ) 79 | { 80 | wallError = 10; 81 | wallDiff = 5; 82 | } 83 | 84 | else if (wallDiff < -50) 85 | { 86 | wallError = -10; 87 | wallDiff = -5; 88 | } 89 | 90 | correction = (wallError * wallP) + (wallDiff * wallD); 91 | wallLastError = wallError; 92 | 93 | if (correction > 50 ) 94 | { 95 | correction = 10; 96 | } 97 | 98 | else if (correction < -50) 99 | { 100 | correction = -10; 101 | } 102 | 103 | leftPwm = leftBase - correction; 104 | rightPwm = rightBase + correction; 105 | 106 | } 107 | 108 | void encoderPid() 109 | { 110 | encoderError = leftEncoder - rightEncoder; 111 | if (encoderError > 50 ) 112 | { 113 | encoderError = 10; 114 | } 115 | else if (encoderError < -50) 116 | { 117 | encoderError = -10; 118 | } 119 | encoderCorrection = float(encoderError * encoderP) + float(encoderLastError * encoderD); 120 | encoderLastError = encoderError; 121 | leftPwm = leftBase - encoderCorrection; 122 | rightPwm = rightBase + encoderCorrection; 123 | forward(); 124 | } 125 | 126 | void wallFollow() 127 | { 128 | tofPid(); 129 | if (tof[0] <= 135 && tof[4] <= 135) 130 | { 131 | state = 0; 132 | if (state != preState) 133 | { 134 | jump = 5; 135 | wallLastError = 0; 136 | } 137 | if (jump>0) 138 | { 139 | //forwardBase(); 140 | encoderPid(); 141 | jump = jump-1; 142 | } 143 | else 144 | { 145 | wallPid(); 146 | forward(); 147 | } 148 | } 149 | 150 | else if (tof[0] > 135 && tof[4] <= 135) 151 | { 152 | state = 1; 153 | if (state != preState) 154 | { 155 | jump = 5; 156 | rightLastError = 0; 157 | } 158 | if (jump>0) 159 | { 160 | encoderPid(); 161 | //forwardBase(); 162 | jump = jump-1; 163 | } 164 | else 165 | { 166 | rightPid(); 167 | forward(); 168 | } 169 | } 170 | 171 | else if (tof[0] <= 135 && tof[4] > 135) 172 | { 173 | state = 2; 174 | if (state != preState) 175 | { 176 | jump = 5; 177 | leftLastError = 0; 178 | } 179 | if (jump>0) 180 | { 181 | encoderPid(); 182 | //forwardBase(); 183 | jump = jump-1; 184 | } 185 | else 186 | { 187 | leftPid(); 188 | forward(); 189 | } 190 | } 191 | else if(tof[0] > 135 && tof[4] > 135) 192 | { 193 | state = 3; 194 | encoderPid(); 195 | //forwardBase(); 196 | wallLastError = 0; 197 | } 198 | preState = state; 199 | } 200 | -------------------------------------------------------------------------------- /piccolaMain/piccolaWallPid.h: -------------------------------------------------------------------------------- 1 | 2 | void leftPid() 3 | { 4 | leftError = 85 - tof[0]; 5 | leftDiff = leftError - leftLastError; 6 | 7 | if (leftDiff > 50 ) 8 | { 9 | leftError = 10; 10 | leftDiff = 5; 11 | } 12 | 13 | else if (leftDiff < -50) 14 | { 15 | leftError = -10; 16 | leftDiff = -5; 17 | } 18 | 19 | correction = (leftError * leftP) + (leftDiff * leftD); 20 | leftLastError = leftError; 21 | 22 | if (correction > 50 ) 23 | { 24 | correction = 15; 25 | } 26 | 27 | else if (correction < -50) 28 | { 29 | correction = -15; 30 | } 31 | 32 | leftPwm = leftBase + correction; 33 | rightPwm = rightBase - correction; 34 | } 35 | 36 | void rightPid() 37 | { 38 | rightError = 80 - tof[4]; 39 | rightDiff = rightError - rightLastError; 40 | if (rightDiff > 50 ) 41 | { 42 | rightError = 10; 43 | rightDiff = 5; 44 | } 45 | 46 | else if (rightDiff < -50) 47 | { 48 | rightError = -10; 49 | rightDiff = -5; 50 | } 51 | 52 | correction = (rightError * rightP) + (rightDiff * rightD); 53 | rightLastError = rightError; 54 | 55 | if (correction > 50 ) 56 | { 57 | correction = 15; 58 | } 59 | 60 | else if (correction < -50) 61 | { 62 | correction = -15; 63 | } 64 | 65 | leftPwm = leftBase - correction; 66 | rightPwm = rightBase + correction; 67 | 68 | } 69 | 70 | 71 | void wallPid() 72 | { 73 | wallError = tof[0] - (tof[4]-8); 74 | rightLastError = 58 - tof[4]; 75 | leftLastError = 50 - tof[0]; 76 | wallDiff = wallError - wallLastError; 77 | 78 | if (wallDiff > 50 ) 79 | { 80 | wallError = 10; 81 | wallDiff = 5; 82 | } 83 | 84 | else if (wallDiff < -50) 85 | { 86 | wallError = -10; 87 | wallDiff = -5; 88 | } 89 | 90 | correction = (wallError * wallP) + (wallDiff * wallD); 91 | wallLastError = wallError; 92 | 93 | if (correction > 50 ) 94 | { 95 | correction = 10; 96 | } 97 | 98 | else if (correction < -50) 99 | { 100 | correction = -10; 101 | } 102 | 103 | leftPwm = leftBase - correction; 104 | rightPwm = rightBase + correction; 105 | 106 | } 107 | 108 | void encoderPid() 109 | { 110 | encoderError = leftEncoder - rightEncoder; 111 | if (encoderError > 50 ) 112 | { 113 | encoderError = 10; 114 | } 115 | else if (encoderError < -50) 116 | { 117 | encoderError = -10; 118 | } 119 | encoderCorrection = float(encoderError * encoderP) + float(encoderLastError * encoderD); 120 | encoderLastError = encoderError; 121 | leftPwm = leftBase - encoderCorrection; 122 | rightPwm = rightBase + encoderCorrection; 123 | forward(); 124 | } 125 | 126 | void wallFollow() 127 | { 128 | tofPid(); 129 | if (tof[0] <= 135 && tof[4] <= 135) 130 | { 131 | state = 0; 132 | if (state != preState) 133 | { 134 | jump = 5; 135 | wallLastError = 0; 136 | } 137 | if (jump>0) 138 | { 139 | //forwardBase(); 140 | encoderPid(); 141 | jump = jump-1; 142 | } 143 | else 144 | { 145 | wallPid(); 146 | forward(); 147 | } 148 | } 149 | 150 | else if (tof[0] > 135 && tof[4] <= 135) 151 | { 152 | state = 1; 153 | if (state != preState) 154 | { 155 | jump = 5; 156 | rightLastError = 0; 157 | } 158 | if (jump>0) 159 | { 160 | encoderPid(); 161 | //forwardBase(); 162 | jump = jump-1; 163 | } 164 | else 165 | { 166 | rightPid(); 167 | forward(); 168 | } 169 | } 170 | 171 | else if (tof[0] <= 135 && tof[4] > 135) 172 | { 173 | state = 2; 174 | if (state != preState) 175 | { 176 | jump = 5; 177 | leftLastError = 0; 178 | } 179 | if (jump>0) 180 | { 181 | encoderPid(); 182 | //forwardBase(); 183 | jump = jump-1; 184 | } 185 | else 186 | { 187 | leftPid(); 188 | forward(); 189 | } 190 | } 191 | else if(tof[0] > 135 && tof[4] > 135) 192 | { 193 | state = 3; 194 | encoderPid(); 195 | //forwardBase(); 196 | wallLastError = 0; 197 | } 198 | preState = state; 199 | } 200 | -------------------------------------------------------------------------------- /code/piccolaTof.h: -------------------------------------------------------------------------------- 1 | #define GPIO1 PA0 2 | #define GPIO2 PA1 3 | #define GPIO3 PA5 4 | #define GPIO4 PA6 5 | #define GPIO5 PA4 6 | 7 | #define tofAddress1 43 8 | #define tofAddress2 44 9 | #define tofAddress3 45 10 | #define tofAddress4 46 11 | #define tofAddress5 47 12 | 13 | VL6180X tof1; 14 | VL6180X tof2; 15 | VL6180X tof3; 16 | VL6180X tof4; 17 | VL6180X tof5; 18 | 19 | void tofSetup() 20 | { 21 | pinMode(GPIO1, OUTPUT); 22 | pinMode(GPIO2, OUTPUT); 23 | pinMode(GPIO3, OUTPUT); 24 | pinMode(GPIO4, OUTPUT); 25 | pinMode(GPIO5, OUTPUT); 26 | digitalWrite(GPIO1, LOW); 27 | digitalWrite(GPIO2, LOW); 28 | digitalWrite(GPIO3, LOW); 29 | digitalWrite(GPIO4, LOW); 30 | digitalWrite(GPIO5, LOW); 31 | 32 | Wire.begin(); 33 | 34 | digitalWrite(GPIO1, HIGH); 35 | delay(10); 36 | tof1.init(); 37 | tof1.configureDefault(); 38 | //tof1.startRangeContinuous(); 39 | tof1.setTimeout(10); 40 | tof1.setAddress(tofAddress1); 41 | 42 | digitalWrite(GPIO2, HIGH); 43 | delay(10); 44 | tof2.init(); 45 | tof2.configureDefault(); 46 | //tof2.startRangeContinuous(); 47 | tof2.setTimeout(10); 48 | tof2.setAddress(tofAddress2); 49 | 50 | digitalWrite(GPIO3, HIGH); 51 | delay(10); 52 | tof3.init(); 53 | tof3.configureDefault(); 54 | //tof3.startRangeContinuous(); 55 | tof3.setTimeout(10); 56 | tof3.setAddress(tofAddress3); 57 | 58 | digitalWrite(GPIO4, HIGH); 59 | delay(10); 60 | tof4.init(); 61 | tof4.configureDefault(); 62 | //tof4.startRangeContinuous(); 63 | tof4.setTimeout(10); 64 | tof4.setAddress(tofAddress4); 65 | 66 | digitalWrite(GPIO5, HIGH); 67 | delay(10); 68 | tof5.init(); 69 | tof5.configureDefault(); 70 | //tof5.startRangeContinuous(); 71 | tof5.setTimeout(10); 72 | tof5.setAddress(tofAddress5); 73 | } 74 | 75 | void tofPid() 76 | { 77 | tof[0] = tof2.readRangeSingleMillimeters(); 78 | tof[2] = tof3.readRangeSingleMillimeters(); 79 | tof[4] = tof4.readRangeSingleMillimeters(); 80 | } 81 | 82 | void tofStart() 83 | { 84 | tof[1] = tof1.readRangeSingleMillimeters(); 85 | tof[2] = tof3.readRangeSingleMillimeters(); 86 | tof[3] = tof5.readRangeSingleMillimeters(); 87 | } 88 | void printTof() 89 | { 90 | 91 | Serial2.print(tof[0]); 92 | Serial2.print(", "); 93 | Serial2.print(tof[2]); 94 | Serial2.print(", "); 95 | Serial2.print(tof[4]); 96 | Serial2.println(); 97 | } 98 | 99 | void checkWallsCell() 100 | { 101 | if (x == 0 && y == 0) 102 | { 103 | time = 0; 104 | while(time <10) 105 | { 106 | tofStart(); 107 | if (tof[2] <= 180) 108 | { 109 | frontWallAvailable = frontWallAvailable + 1; 110 | } 111 | else 112 | { 113 | frontWallAvailable = frontWallAvailable - 1; 114 | } 115 | if (tof[1] <= 150) 116 | { 117 | leftWallAvailable= leftWallAvailable + 1; 118 | } 119 | else 120 | { 121 | leftWallAvailable= leftWallAvailable - 1; 122 | } 123 | if (tof[3] <= 150) 124 | { 125 | rightWallAvailable= rightWallAvailable + 1; 126 | } 127 | else 128 | { 129 | rightWallAvailable= rightWallAvailable - 1; 130 | } 131 | time = time + 1; 132 | } 133 | } 134 | if (frontWallAvailable >=0 ) 135 | { 136 | cellWalls[1] = 0; 137 | F = true; 138 | //frontWallAvailable = 0; 139 | } 140 | else 141 | { 142 | cellWalls[1] = 1; 143 | F = false; 144 | //frontWallAvailable = 0; 145 | } 146 | if (leftWallAvailable >= 0) 147 | { 148 | cellWalls[0] = 1; 149 | L = true; 150 | //leftWallAvailable = 0; 151 | } 152 | else 153 | { 154 | cellWalls[0] = 0; 155 | L = false; 156 | //leftWallAvailable = 0; 157 | } 158 | if (rightWallAvailable >= 0) 159 | { 160 | cellWalls[2] = 1; 161 | R = true; 162 | //rightWallAvailable = 0; 163 | } 164 | else 165 | { 166 | cellWalls[2] = 0; 167 | R = false; 168 | //rightWallAvailable = 0; 169 | } 170 | 171 | } 172 | 173 | void printWallState() 174 | { 175 | for (int i=0; i<3; i++) 176 | { 177 | Serial2.print(cellWalls[i]); 178 | Serial2.print(" "); 179 | } 180 | Serial2.println(""); 181 | } 182 | -------------------------------------------------------------------------------- /piccolaMain/piccolaTof.h: -------------------------------------------------------------------------------- 1 | #define GPIO1 PA0 2 | #define GPIO2 PA1 3 | #define GPIO3 PA5 4 | #define GPIO4 PA6 5 | #define GPIO5 PA4 6 | 7 | #define tofAddress1 43 8 | #define tofAddress2 44 9 | #define tofAddress3 45 10 | #define tofAddress4 46 11 | #define tofAddress5 47 12 | 13 | VL6180X tof1; 14 | VL6180X tof2; 15 | VL6180X tof3; 16 | VL6180X tof4; 17 | VL6180X tof5; 18 | 19 | void tofSetup() 20 | { 21 | pinMode(GPIO1, OUTPUT); 22 | pinMode(GPIO2, OUTPUT); 23 | pinMode(GPIO3, OUTPUT); 24 | pinMode(GPIO4, OUTPUT); 25 | pinMode(GPIO5, OUTPUT); 26 | digitalWrite(GPIO1, LOW); 27 | digitalWrite(GPIO2, LOW); 28 | digitalWrite(GPIO3, LOW); 29 | digitalWrite(GPIO4, LOW); 30 | digitalWrite(GPIO5, LOW); 31 | 32 | Wire.begin(); 33 | 34 | digitalWrite(GPIO1, HIGH); 35 | delay(10); 36 | tof1.init(); 37 | tof1.configureDefault(); 38 | //tof1.startRangeContinuous(); 39 | tof1.setTimeout(10); 40 | tof1.setAddress(tofAddress1); 41 | 42 | digitalWrite(GPIO2, HIGH); 43 | delay(10); 44 | tof2.init(); 45 | tof2.configureDefault(); 46 | //tof2.startRangeContinuous(); 47 | tof2.setTimeout(10); 48 | tof2.setAddress(tofAddress2); 49 | 50 | digitalWrite(GPIO3, HIGH); 51 | delay(10); 52 | tof3.init(); 53 | tof3.configureDefault(); 54 | //tof3.startRangeContinuous(); 55 | tof3.setTimeout(10); 56 | tof3.setAddress(tofAddress3); 57 | 58 | digitalWrite(GPIO4, HIGH); 59 | delay(10); 60 | tof4.init(); 61 | tof4.configureDefault(); 62 | //tof4.startRangeContinuous(); 63 | tof4.setTimeout(10); 64 | tof4.setAddress(tofAddress4); 65 | 66 | digitalWrite(GPIO5, HIGH); 67 | delay(10); 68 | tof5.init(); 69 | tof5.configureDefault(); 70 | //tof5.startRangeContinuous(); 71 | tof5.setTimeout(10); 72 | tof5.setAddress(tofAddress5); 73 | } 74 | 75 | void tofPid() 76 | { 77 | tof[0] = tof2.readRangeSingleMillimeters(); 78 | tof[2] = tof3.readRangeSingleMillimeters(); 79 | tof[4] = tof4.readRangeSingleMillimeters(); 80 | } 81 | 82 | void tofStart() 83 | { 84 | tof[1] = tof1.readRangeSingleMillimeters(); 85 | tof[2] = tof3.readRangeSingleMillimeters(); 86 | tof[3] = tof5.readRangeSingleMillimeters(); 87 | } 88 | void printTof() 89 | { 90 | 91 | Serial2.print(tof[0]); 92 | Serial2.print(", "); 93 | Serial2.print(tof[2]); 94 | Serial2.print(", "); 95 | Serial2.print(tof[4]); 96 | Serial2.println(); 97 | } 98 | 99 | void checkWallsCell() 100 | { 101 | if (x == 0 && y == 0) 102 | { 103 | time = 0; 104 | while(time <10) 105 | { 106 | tofStart(); 107 | if (tof[2] <= 180) 108 | { 109 | frontWallAvailable = frontWallAvailable + 1; 110 | } 111 | else 112 | { 113 | frontWallAvailable = frontWallAvailable - 1; 114 | } 115 | if (tof[1] <= 150) 116 | { 117 | leftWallAvailable= leftWallAvailable + 1; 118 | } 119 | else 120 | { 121 | leftWallAvailable= leftWallAvailable - 1; 122 | } 123 | if (tof[3] <= 150) 124 | { 125 | rightWallAvailable= rightWallAvailable + 1; 126 | } 127 | else 128 | { 129 | rightWallAvailable= rightWallAvailable - 1; 130 | } 131 | time = time + 1; 132 | } 133 | } 134 | if (frontWallAvailable >=0 ) 135 | { 136 | cellWalls[1] = 0; 137 | F = true; 138 | //frontWallAvailable = 0; 139 | } 140 | else 141 | { 142 | cellWalls[1] = 1; 143 | F = false; 144 | //frontWallAvailable = 0; 145 | } 146 | if (leftWallAvailable >= 0) 147 | { 148 | cellWalls[0] = 1; 149 | L = true; 150 | //leftWallAvailable = 0; 151 | } 152 | else 153 | { 154 | cellWalls[0] = 0; 155 | L = false; 156 | //leftWallAvailable = 0; 157 | } 158 | if (rightWallAvailable >= 0) 159 | { 160 | cellWalls[2] = 1; 161 | R = true; 162 | //rightWallAvailable = 0; 163 | } 164 | else 165 | { 166 | cellWalls[2] = 0; 167 | R = false; 168 | //rightWallAvailable = 0; 169 | } 170 | 171 | } 172 | 173 | void printWallState() 174 | { 175 | for (int i=0; i<3; i++) 176 | { 177 | Serial2.print(cellWalls[i]); 178 | Serial2.print(" "); 179 | } 180 | Serial2.println(""); 181 | } 182 | -------------------------------------------------------------------------------- /piccolaPowerSupply/Project Logs for piccolaPowerSupply/piccolaPowerPCB PCB ECO 7-17-2019 10-42-54 PM.LOG: -------------------------------------------------------------------------------- 1 | Added Component: Designator=buck01 IN(HDR1X2) 2 | Add component. Clean all parameters for all variants 3 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 4 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 5 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 6 | Added Component: Designator=buck01 OUT(HDR1X2) 7 | Add component. Clean all parameters for all variants 8 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 9 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 10 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 11 | Added Component: Designator=buck02 IN(HDR1X2) 12 | Add component. Clean all parameters for all variants 13 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 14 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 15 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 16 | Added Component: Designator=buck02 OUT(HDR1X2) 17 | Add component. Clean all parameters for all variants 18 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 19 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 20 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 21 | Added Component: Designator=buck03 IN(HDR1X2) 22 | Add component. Clean all parameters for all variants 23 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 24 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 25 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 26 | Added Component: Designator=buck03 OUT(HDR1X2) 27 | Add component. Clean all parameters for all variants 28 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 29 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 30 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 31 | Added Component: Designator=D?(LED-1) 32 | Add component. Clean all parameters for all variants 33 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "08-Jul-2005"; VariantName = "[No Variations]" 34 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Mech Layer 1 removed."; VariantName = "[No Variations]" 35 | Add component (AddParameter): Name = "PackageDescription"; Value = "LED; 2 Leads"; VariantName = "[No Variations]" 36 | Add component (AddParameter): Name = "PackageReference"; Value = "LED-1"; VariantName = "[No Variations]" 37 | Add component (AddParameter): Name = "Published"; Value = "8-Jun-2000"; VariantName = "[No Variations]" 38 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 39 | Added Component: Designator=R?(AXIAL-0.3) 40 | Add component. Clean all parameters for all variants 41 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 42 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 43 | Add component (AddParameter): Name = "PackageDescription"; Value = "Axial Device, Thru-Hole; 2 Leads; 0.3 in Pin Spacing"; VariantName = "[No Variations]" 44 | Add component (AddParameter): Name = "PackageReference"; Value = "AXIAL-0.3"; VariantName = "[No Variations]" 45 | Add component (AddParameter): Name = "Published"; Value = "8-Jun-2000"; VariantName = "[No Variations]" 46 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 47 | Add component (AddParameter): Name = "Value"; Value = "1K"; VariantName = "[No Variations]" 48 | Added Component: Designator=supply(HDR1X2) 49 | Add component. Clean all parameters for all variants 50 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 51 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 52 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 53 | Added Pin To Net: NetName=GND Pin=buck01 IN-2 54 | Added Pin To Net: NetName=GND Pin=buck02 IN-2 55 | Added Pin To Net: NetName=GND Pin=buck03 IN-2 56 | Added Pin To Net: NetName=GND Pin=D?-2 57 | Added Pin To Net: NetName=GND Pin=supply-2 58 | Added Net: Name=GND 59 | Added Pin To Net: NetName=NetD?_1 Pin=D?-1 60 | Added Pin To Net: NetName=NetD?_1 Pin=R?-2 61 | Added Net: Name=NetD?_1 62 | Added Pin To Net: NetName=VCC Pin=buck01 IN-1 63 | Added Pin To Net: NetName=VCC Pin=buck02 IN-1 64 | Added Pin To Net: NetName=VCC Pin=buck03 IN-1 65 | Added Pin To Net: NetName=VCC Pin=supply-1 66 | Added Net: Name=VCC 67 | Added Class: Name=piccolaPowerSupply 68 | Added Room: Name=piccolaPowerSupply 69 | -------------------------------------------------------------------------------- /code/piccolaTurns.h: -------------------------------------------------------------------------------- 1 | void countLeftOut1() 2 | { 3 | leftEncoder = leftEncoder + 1; 4 | } 5 | 6 | void countRightOut1() 7 | { 8 | rightEncoder = rightEncoder + 1; 9 | } 10 | 11 | void turnBack() 12 | { 13 | encoderLeftCount = 0; 14 | encoderRightCount = 0; 15 | leftEncoder = 0; 16 | rightEncoder = 0; 17 | encoderRightCount= encoderRightCount + 100; 18 | encoderLeftCount= encoderLeftCount + 100; 19 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 20 | { 21 | dif = leftEncoder - encoderLeftCount + 100; 22 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 23 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 24 | turnRight(); 25 | } 26 | rightBase=176; 27 | leftBase=180; 28 | encoderRightCount= encoderRightCount + 703; 29 | encoderLeftCount= encoderLeftCount + 703; 30 | while(rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 31 | { 32 | turnRight(); 33 | } 34 | encoderRightCount= encoderRightCount + 100; 35 | encoderLeftCount= encoderLeftCount + 100; 36 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 37 | { 38 | dif = leftEncoder - encoderLeftCount + 100; 39 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 40 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 41 | turnRight(); 42 | } 43 | brake(); 44 | leftBase = 180; 45 | rightBase = 176; 46 | encoderLeftCount = 0; 47 | encoderRightCount = 0; 48 | leftEncoder = 0; 49 | rightEncoder = 0; 50 | } 51 | 52 | void rightAboutTurn() 53 | { 54 | encoderLeftCount = 0; 55 | encoderRightCount = 0; 56 | leftEncoder = 0; 57 | rightEncoder = 0; 58 | encoderRightCount= encoderRightCount + 100; 59 | encoderLeftCount= encoderLeftCount + 100; 60 | rightBase = 70; 61 | leftBase = 70; 62 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 63 | { 64 | dif = leftEncoder - encoderLeftCount + 100; 65 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 66 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 67 | turnRight(); 68 | } 69 | rightBase=176; 70 | leftBase=180; 71 | encoderRightCount= encoderRightCount + 230; 72 | encoderLeftCount= encoderLeftCount + 230; 73 | while(rightEncoder <= encoderRightCount) 74 | { 75 | turnRight(); 76 | } 77 | encoderRightCount= encoderRightCount + 100; 78 | encoderLeftCount= encoderLeftCount + 100; 79 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 80 | { 81 | dif = leftEncoder - encoderLeftCount + 100; 82 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 83 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 84 | turnRight(); 85 | } 86 | brake(); 87 | leftBase = 180; 88 | rightBase = 176; 89 | encoderLeftCount = 0; 90 | encoderRightCount = 0; 91 | leftEncoder = 0; 92 | rightEncoder = 0; 93 | } 94 | 95 | void leftAboutTurn() 96 | { 97 | encoderLeftCount = 0; 98 | encoderRightCount = 0; 99 | leftEncoder = 0; 100 | rightEncoder = 0; 101 | rightBase = 70; 102 | leftBase = 70; 103 | encoderRightCount= encoderRightCount + 100; 104 | encoderLeftCount= encoderLeftCount + 100; 105 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 106 | { 107 | dif = leftEncoder - encoderLeftCount + 100; 108 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 109 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 110 | turnLeft(); 111 | } 112 | rightBase=176; 113 | leftBase=180; 114 | encoderRightCount= encoderRightCount + 240; 115 | encoderLeftCount= encoderLeftCount + 240; 116 | while(rightEncoder <= encoderRightCount) 117 | { 118 | turnLeft(); 119 | } 120 | 121 | encoderRightCount= encoderRightCount + 100; 122 | encoderLeftCount= encoderLeftCount + 100; 123 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 124 | { 125 | dif = leftEncoder - encoderLeftCount + 100; 126 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 127 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 128 | turnLeft(); 129 | } 130 | brake(); 131 | leftBase = 180; 132 | rightBase = 176; 133 | encoderLeftCount = 0; 134 | encoderRightCount = 0; 135 | leftEncoder = 0; 136 | rightEncoder = 0; 137 | } 138 | 139 | void rightHalfTurn() 140 | { 141 | encoderLeftCount = 0; 142 | encoderRightCount = 0; 143 | leftEncoder = 0; 144 | rightEncoder = 0; 145 | encoderRightCount= encoderRightCount + 100; 146 | encoderLeftCount= encoderLeftCount + 100; 147 | rightBase = 70; 148 | leftBase = 70; 149 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 150 | { 151 | dif = leftEncoder - encoderLeftCount + 100; 152 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 153 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 154 | turnRight(); 155 | } 156 | rightBase=176; 157 | leftBase=180; 158 | encoderRightCount= encoderRightCount + 1; 159 | encoderLeftCount= encoderLeftCount + 1; 160 | while(rightEncoder <= encoderRightCount) 161 | { 162 | turnRight(); 163 | } 164 | encoderRightCount= encoderRightCount + 100; 165 | encoderLeftCount= encoderLeftCount + 100; 166 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 167 | { 168 | dif = leftEncoder - encoderLeftCount + 100; 169 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 170 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 171 | turnRight(); 172 | } 173 | brake(); 174 | leftBase = 180; 175 | rightBase = 176; 176 | encoderLeftCount = 0; 177 | encoderRightCount = 0; 178 | leftEncoder = 0; 179 | rightEncoder = 0; 180 | } 181 | 182 | -------------------------------------------------------------------------------- /piccolaMain/piccolaTurns.h: -------------------------------------------------------------------------------- 1 | void countLeftOut1() 2 | { 3 | leftEncoder = leftEncoder + 1; 4 | } 5 | 6 | void countRightOut1() 7 | { 8 | rightEncoder = rightEncoder + 1; 9 | } 10 | 11 | void turnBack() 12 | { 13 | encoderLeftCount = 0; 14 | encoderRightCount = 0; 15 | leftEncoder = 0; 16 | rightEncoder = 0; 17 | encoderRightCount= encoderRightCount + 100; 18 | encoderLeftCount= encoderLeftCount + 100; 19 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 20 | { 21 | dif = leftEncoder - encoderLeftCount + 100; 22 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 23 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 24 | turnRight(); 25 | } 26 | rightBase=176; 27 | leftBase=180; 28 | encoderRightCount= encoderRightCount + 725; 29 | encoderLeftCount= encoderLeftCount + 725; 30 | while(rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 31 | { 32 | turnRight(); 33 | } 34 | encoderRightCount= encoderRightCount + 100; 35 | encoderLeftCount= encoderLeftCount + 100; 36 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 37 | { 38 | dif = leftEncoder - encoderLeftCount + 100; 39 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 40 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 41 | turnRight(); 42 | } 43 | brake(); 44 | leftBase = 180; 45 | rightBase = 176; 46 | encoderLeftCount = 0; 47 | encoderRightCount = 0; 48 | leftEncoder = 0; 49 | rightEncoder = 0; 50 | } 51 | 52 | void rightAboutTurn() 53 | { 54 | encoderLeftCount = 0; 55 | encoderRightCount = 0; 56 | leftEncoder = 0; 57 | rightEncoder = 0; 58 | encoderRightCount= encoderRightCount + 100; 59 | encoderLeftCount= encoderLeftCount + 100; 60 | rightBase = 70; 61 | leftBase = 70; 62 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 63 | { 64 | dif = leftEncoder - encoderLeftCount + 100; 65 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 66 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 67 | turnRight(); 68 | } 69 | rightBase=176; 70 | leftBase=180; 71 | encoderRightCount= encoderRightCount + 240; 72 | encoderLeftCount= encoderLeftCount + 240; 73 | while(rightEncoder <= encoderRightCount) 74 | { 75 | turnRight(); 76 | } 77 | encoderRightCount= encoderRightCount + 100; 78 | encoderLeftCount= encoderLeftCount + 100; 79 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 80 | { 81 | dif = leftEncoder - encoderLeftCount + 100; 82 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 83 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 84 | turnRight(); 85 | } 86 | brake(); 87 | leftBase = 180; 88 | rightBase = 176; 89 | encoderLeftCount = 0; 90 | encoderRightCount = 0; 91 | leftEncoder = 0; 92 | rightEncoder = 0; 93 | } 94 | 95 | void leftAboutTurn() 96 | { 97 | encoderLeftCount = 0; 98 | encoderRightCount = 0; 99 | leftEncoder = 0; 100 | rightEncoder = 0; 101 | rightBase = 70; 102 | leftBase = 70; 103 | encoderRightCount= encoderRightCount + 100; 104 | encoderLeftCount= encoderLeftCount + 100; 105 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 106 | { 107 | dif = leftEncoder - encoderLeftCount + 100; 108 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 109 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 110 | turnLeft(); 111 | } 112 | rightBase=176; 113 | leftBase=180; 114 | encoderRightCount= encoderRightCount + 255; 115 | encoderLeftCount= encoderLeftCount + 255; 116 | while(rightEncoder <= encoderRightCount) 117 | { 118 | turnLeft(); 119 | } 120 | 121 | encoderRightCount= encoderRightCount + 100; 122 | encoderLeftCount= encoderLeftCount + 100; 123 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 124 | { 125 | dif = leftEncoder - encoderLeftCount + 100; 126 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 127 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 128 | turnLeft(); 129 | } 130 | brake(); 131 | leftBase = 180; 132 | rightBase = 176; 133 | encoderLeftCount = 0; 134 | encoderRightCount = 0; 135 | leftEncoder = 0; 136 | rightEncoder = 0; 137 | } 138 | 139 | void rightHalfTurn() 140 | { 141 | encoderLeftCount = 0; 142 | encoderRightCount = 0; 143 | leftEncoder = 0; 144 | rightEncoder = 0; 145 | encoderRightCount= encoderRightCount + 100; 146 | encoderLeftCount= encoderLeftCount + 100; 147 | rightBase = 70; 148 | leftBase = 70; 149 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 150 | { 151 | dif = leftEncoder - encoderLeftCount + 100; 152 | rightBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 153 | leftBase = int(70+110/(1+pow(2.73,((50-dif)*0.05)))); 154 | turnRight(); 155 | } 156 | rightBase=176; 157 | leftBase=180; 158 | encoderRightCount= encoderRightCount + 1; 159 | encoderLeftCount= encoderLeftCount + 1; 160 | while(rightEncoder <= encoderRightCount) 161 | { 162 | turnRight(); 163 | } 164 | encoderRightCount= encoderRightCount + 100; 165 | encoderLeftCount= encoderLeftCount + 100; 166 | while (rightEncoder <= encoderRightCount || leftEncoder <= encoderLeftCount) 167 | { 168 | dif = leftEncoder - encoderLeftCount + 100; 169 | rightBase = int(176-110/(1+pow(2.73,((50-dif)*0.05)))); 170 | leftBase = int(180-110/(1+pow(2.73,((50-dif)*0.05)))); 171 | turnRight(); 172 | } 173 | brake(); 174 | leftBase = 180; 175 | rightBase = 176; 176 | encoderLeftCount = 0; 177 | encoderRightCount = 0; 178 | leftEncoder = 0; 179 | rightEncoder = 0; 180 | } 181 | 182 | -------------------------------------------------------------------------------- /Amaya/piccolaMain/piccolaTof.h: -------------------------------------------------------------------------------- 1 | #define GPIO1 PA0 2 | #define GPIO2 PA1 3 | #define GPIO3 PA5 4 | #define GPIO4 PA6 5 | #define GPIO5 PA4 6 | 7 | #define tofAddress1 43 8 | #define tofAddress2 44 9 | #define tofAddress3 45 10 | #define tofAddress4 46 11 | #define tofAddress5 47 12 | 13 | VL6180X tof1; 14 | VL6180X tof2; 15 | VL6180X tof3; 16 | VL6180X tof4; 17 | VL6180X tof5; 18 | 19 | void tofSetup() 20 | { 21 | pinMode(GPIO1, OUTPUT); 22 | pinMode(GPIO2, OUTPUT); 23 | pinMode(GPIO3, OUTPUT); 24 | pinMode(GPIO4, OUTPUT); 25 | pinMode(GPIO5, OUTPUT); 26 | digitalWrite(GPIO1, LOW); 27 | digitalWrite(GPIO2, LOW); 28 | digitalWrite(GPIO3, LOW); 29 | digitalWrite(GPIO4, LOW); 30 | digitalWrite(GPIO5, LOW); 31 | 32 | Wire.begin(); 33 | 34 | digitalWrite(GPIO1, HIGH); 35 | delay(10); 36 | tof1.init(); 37 | tof1.configureDefault(); 38 | tof1.setTimeout(50); 39 | tof1.setAddress(tofAddress1); 40 | 41 | digitalWrite(GPIO2, HIGH); 42 | delay(10); 43 | tof2.init(); 44 | tof2.configureDefault(); 45 | //Sensor1.startRangeContinuous(); 46 | tof2.setTimeout(50); 47 | tof2.setAddress(tofAddress2); 48 | 49 | digitalWrite(GPIO3, HIGH); 50 | delay(10); 51 | tof3.init(); 52 | tof3.configureDefault(); 53 | //Sensor1.startRangeContinuous(); 54 | tof3.setTimeout(50); 55 | tof3.setAddress(tofAddress3); 56 | 57 | digitalWrite(GPIO4, HIGH); 58 | delay(10); 59 | tof4.init(); 60 | tof4.configureDefault(); 61 | //Sensor1.startRangeContinuous(); 62 | tof4.setTimeout(50); 63 | tof4.setAddress(tofAddress4); 64 | 65 | digitalWrite(GPIO5, HIGH); 66 | delay(10); 67 | tof5.init(); 68 | tof5.configureDefault(); 69 | //Sensor1.startRangeContinuous(); 70 | tof5.setTimeout(50); 71 | tof5.setAddress(tofAddress5); 72 | } 73 | 74 | void tofCell() 75 | { 76 | 77 | tof[1] = tof2.readRangeSingleMillimeters(); 78 | tof[2] = tof3.readRangeSingleMillimeters(); 79 | tof[3] = tof4.readRangeSingleMillimeters(); 80 | 81 | } 82 | 83 | void tofPid() 84 | { 85 | tof[0] = tof1.readRangeSingleMillimeters(); 86 | tof[4] = tof5.readRangeSingleMillimeters(); 87 | } 88 | 89 | void printTof() 90 | { 91 | /* 92 | Serial2.print(tof[0]); 93 | Serial2.print(", "); 94 | */ 95 | Serial2.print(tof[1]); 96 | Serial2.print(", "); 97 | Serial2.print(tof[2]); 98 | Serial2.print(", "); 99 | Serial2.print(tof[3]); 100 | Serial2.print(", "); 101 | /* 102 | Serial2.print(tof[4]); 103 | */ 104 | Serial2.println(); 105 | 106 | } 107 | 108 | 109 | 110 | 111 | void checkWallsPid() 112 | { 113 | wallAvailable[2] ={0}; 114 | //check left wall availability 115 | if (tof[0] <= 150) 116 | { 117 | wallAvailable[0] = 1; 118 | } 119 | else 120 | { 121 | tofPid(); 122 | if (tof[0] <= 150) 123 | { 124 | wallAvailable[0] = 1; 125 | } 126 | else 127 | { 128 | tofPid(); 129 | if (tof[0] <= 150) 130 | { 131 | wallAvailable[0] = 1; 132 | } 133 | else 134 | { 135 | wallAvailable[0] = 0; 136 | } 137 | 138 | } 139 | 140 | } 141 | 142 | //check right wall availability 143 | if (tof[4] <= 150) 144 | { 145 | wallAvailable[1] = 1; 146 | } 147 | else 148 | { 149 | tofPid(); 150 | if (tof[4] <= 150) 151 | { 152 | wallAvailable[1] = 1; 153 | } 154 | else 155 | { 156 | tofPid(); 157 | if (tof[4] <= 150) 158 | { 159 | wallAvailable[1] = 1; 160 | } 161 | else 162 | { 163 | wallAvailable[1] = 0; 164 | } 165 | 166 | } 167 | 168 | } 169 | 170 | } 171 | 172 | void checkWallsCell() 173 | { 174 | //check front wall availability 175 | if (tof[2] > 40) 176 | { 177 | cellWalls[1] = 0; 178 | } 179 | else 180 | { 181 | tofCell(); 182 | if (tof[2] > 40) 183 | { 184 | cellWalls[1] = 0; 185 | } 186 | else 187 | { 188 | tofCell(); 189 | if (tof[2] > 40) 190 | { 191 | cellWalls[1] = 0; 192 | } 193 | else 194 | { 195 | cellWalls[1] = 1; 196 | } 197 | 198 | } 199 | 200 | } 201 | 202 | //check left-forward wall availability 203 | if (tof[1] <= 200) 204 | { 205 | cellWalls[0] = 1; 206 | } 207 | else 208 | { 209 | tofCell(); 210 | if (tof[1] <= 200) 211 | { 212 | cellWalls[0] = 1; 213 | } 214 | else 215 | { 216 | tofCell(); 217 | if (tof[1] <= 200) 218 | { 219 | cellWalls[0] = 1; 220 | } 221 | else 222 | { 223 | cellWalls[0] = 0; 224 | } 225 | 226 | } 227 | 228 | } 229 | 230 | //check left-forward wall availability 231 | if (tof[3] <= 200) 232 | { 233 | cellWalls[2] = 1; 234 | } 235 | else 236 | { 237 | tofCell(); 238 | if (tof[3] <= 200) 239 | { 240 | cellWalls[2] = 1; 241 | } 242 | else 243 | { 244 | tofCell(); 245 | if (tof[3] <= 200) 246 | { 247 | cellWalls[2] = 1; 248 | } 249 | else 250 | { 251 | cellWalls[2] = 0; 252 | } 253 | 254 | } 255 | 256 | } 257 | } 258 | 259 | 260 | 261 | -------------------------------------------------------------------------------- /code/piccolaMoves.h: -------------------------------------------------------------------------------- 1 | void mazeStart() 2 | { 3 | selectMode = 0; 4 | buzz(); 5 | while(1) 6 | { 7 | tofStart(); 8 | tofStart(); 9 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 10 | { 11 | buzzTwice(); 12 | selectMode = 1; 13 | delay(1000); 14 | break; 15 | } 16 | } 17 | buzz(); 18 | time = 0; 19 | while(time < 50) 20 | { 21 | tofStart(); 22 | tofStart(); 23 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 24 | { 25 | buzzTwice(); 26 | selectMode = 2; 27 | delay(1000); 28 | break; 29 | } 30 | time = time + 1; 31 | } 32 | 33 | if (selectMode == 2) 34 | { 35 | buzz(); 36 | time = 0; 37 | while(time < 50) 38 | { 39 | tofStart(); 40 | tofStart(); 41 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 42 | { 43 | buzzTwice(); 44 | selectMode = 3; 45 | delay(1000); 46 | break; 47 | } 48 | time = time + 1; 49 | } 50 | } 51 | buzzDone(); 52 | } 53 | 54 | void cellForward() 55 | { 56 | leftWallAvailable = 0; 57 | rightWallAvailable = 0; 58 | frontWallAvailable = 0; 59 | encoderLeftCount = encoderLeftCount + 1123; 60 | encoderRightCount = encoderRightCount + 1123; 61 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 62 | { 63 | wallFollow(); 64 | } 65 | encoderLeftCount = encoderLeftCount + 150; 66 | encoderRightCount = encoderRightCount + 150; 67 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 68 | { 69 | wallFollow(); 70 | //encoderPid(); 71 | if (tof[2] <= 180) 72 | { 73 | frontWallAvailable = frontWallAvailable + 1; 74 | } 75 | else 76 | { 77 | frontWallAvailable = frontWallAvailable - 1; 78 | } 79 | if (tof[0] <= 150) 80 | { 81 | leftWallAvailable= leftWallAvailable + 1; 82 | } 83 | else 84 | { 85 | leftWallAvailable= leftWallAvailable - 1; 86 | } 87 | if (tof[4] <= 150) 88 | { 89 | rightWallAvailable= rightWallAvailable + 1; 90 | } 91 | else 92 | { 93 | rightWallAvailable= rightWallAvailable - 1; 94 | } 95 | } 96 | } 97 | 98 | void cellFastForward() 99 | { 100 | leftBase = 180; 101 | rightBase = 176; 102 | leftWallAvailable = 0; 103 | rightWallAvailable = 0; 104 | frontWallAvailable = 0; 105 | encoderLeftCount = encoderLeftCount + 1273; 106 | encoderRightCount = encoderRightCount + 1273; 107 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 108 | { 109 | wallFollow(); 110 | if (tof[2] <=240) 111 | { 112 | while (tof[2] > 150) 113 | { 114 | wallFollow(); 115 | } 116 | leftEncoder = encoderLeftCount + 1; 117 | rightEncoder = encoderRightCount + 1; 118 | test = 2; 119 | break; 120 | } 121 | } 122 | } 123 | 124 | 125 | void leftSmoothTurn() 126 | { 127 | leftBase = 180; 128 | rightBase = 176; 129 | encoderLeftCount = 0; 130 | encoderRightCount = 0; 131 | leftEncoder = 0; 132 | rightEncoder = 0; 133 | encoderRightCount = encoderRightCount + 300; 134 | while (rightEncoder <= encoderRightCount) 135 | { 136 | dif = rightEncoder - encoderRightCount + 300; 137 | rightBase = rightBase + int(dif * 0.1); 138 | leftBase = leftBase - int(dif/3); 139 | forwardBase(); 140 | } 141 | rightBase = 210; 142 | leftBase = 70; 143 | encoderLeftCount = 0; 144 | encoderRightCount = 0; 145 | leftEncoder = 0; 146 | rightEncoder = 0; 147 | encoderRightCount = encoderRightCount + 860; 148 | while (rightEncoder <= encoderRightCount) 149 | { 150 | //rightForwardBase(); 151 | //leftBrake(); 152 | forwardBase(); 153 | } 154 | encoderLeftCount = 0; 155 | encoderRightCount = 0; 156 | leftEncoder = 0; 157 | rightEncoder = 0; 158 | encoderRightCount = encoderRightCount + 300; 159 | while (rightEncoder <= encoderRightCount) 160 | { 161 | dif = rightEncoder - encoderRightCount + 300; 162 | rightBase = rightBase - int(dif * 0.1); 163 | leftBase = leftBase + int(dif/3); 164 | forwardBase(); 165 | } 166 | leftBase = 180; 167 | rightBase = 176; 168 | encoderRightCount = encoderRightCount + 200; 169 | while (rightEncoder <= encoderRightCount) 170 | { 171 | wallFollow(); 172 | } 173 | } 174 | 175 | 176 | void rightSmoothTurn() 177 | { 178 | leftBase = 180; 179 | rightBase = 176; 180 | encoderLeftCount = 0; 181 | encoderRightCount = 0; 182 | leftEncoder = 0; 183 | rightEncoder = 0; 184 | encoderLeftCount = encoderLeftCount + 300; 185 | while (leftEncoder <= encoderLeftCount) 186 | { 187 | dif = leftEncoder - encoderLeftCount + 300; 188 | leftBase = leftBase + int(dif * 0.1); 189 | rightBase = rightBase - int(dif/3); 190 | forwardBase(); 191 | } 192 | leftBase = 210; 193 | rightBase = 70; 194 | encoderLeftCount = 0; 195 | encoderRightCount = 0; 196 | leftEncoder = 0; 197 | rightEncoder = 0; 198 | encoderLeftCount = encoderLeftCount + 840; 199 | while (leftEncoder <= encoderLeftCount) 200 | { 201 | //leftForwardBase(); 202 | //rightBrake(); 203 | forwardBase(); 204 | } 205 | encoderLeftCount = encoderLeftCount + 300; 206 | while (leftEncoder <= encoderLeftCount) 207 | { 208 | dif = leftEncoder - encoderLeftCount + 300; 209 | leftBase = leftBase - int(dif * 0.1); 210 | rightBase = rightBase + int(dif/3); 211 | forwardBase(); 212 | } 213 | leftBase = 180; 214 | rightBase = 176; 215 | encoderLeftCount = encoderLeftCount + 200; 216 | while (leftEncoder <= encoderLeftCount) 217 | { 218 | wallFollow(); 219 | } 220 | } 221 | 222 | 223 | 224 | void rightTurn() 225 | { 226 | cellBrake(); 227 | rightAboutTurn(); 228 | cellStart(); 229 | } 230 | 231 | void leftTurn() 232 | { 233 | cellBrake(); 234 | leftAboutTurn(); 235 | cellStart(); 236 | } 237 | void cellBack() 238 | { 239 | cellBrake(); 240 | turnBack(); 241 | cellStart(); 242 | } 243 | -------------------------------------------------------------------------------- /piccolaMain/piccolaMoves.h: -------------------------------------------------------------------------------- 1 | void mazeStart() 2 | { 3 | selectMode = 0; 4 | buzz(); 5 | while(1) 6 | { 7 | tofStart(); 8 | tofStart(); 9 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 10 | { 11 | buzzTwice(); 12 | selectMode = 1; 13 | delay(1000); 14 | break; 15 | } 16 | } 17 | buzz(); 18 | time = 0; 19 | while(time < 50) 20 | { 21 | tofStart(); 22 | tofStart(); 23 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 24 | { 25 | buzzTwice(); 26 | selectMode = 2; 27 | delay(1000); 28 | break; 29 | } 30 | time = time + 1; 31 | } 32 | 33 | if (selectMode == 2) 34 | { 35 | buzz(); 36 | time = 0; 37 | while(time < 50) 38 | { 39 | tofStart(); 40 | tofStart(); 41 | if (tof[1] <= 120 && tof[2] <= 120 && tof[3] <= 120) 42 | { 43 | buzzTwice(); 44 | selectMode = 3; 45 | delay(1000); 46 | break; 47 | } 48 | time = time + 1; 49 | } 50 | } 51 | buzzDone(); 52 | } 53 | 54 | void cellForward() 55 | { 56 | leftWallAvailable = 0; 57 | rightWallAvailable = 0; 58 | frontWallAvailable = 0; 59 | encoderLeftCount = encoderLeftCount + 1123; 60 | encoderRightCount = encoderRightCount + 1123; 61 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 62 | { 63 | wallFollow(); 64 | } 65 | encoderLeftCount = encoderLeftCount + 150; 66 | encoderRightCount = encoderRightCount + 150; 67 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 68 | { 69 | wallFollow(); 70 | //encoderPid(); 71 | if (tof[2] <= 180) 72 | { 73 | frontWallAvailable = frontWallAvailable + 1; 74 | } 75 | else 76 | { 77 | frontWallAvailable = frontWallAvailable - 1; 78 | } 79 | if (tof[0] <= 160) 80 | { 81 | leftWallAvailable= leftWallAvailable + 1; 82 | } 83 | else 84 | { 85 | leftWallAvailable= leftWallAvailable - 1; 86 | } 87 | if (tof[4] <= 160) 88 | { 89 | rightWallAvailable= rightWallAvailable + 1; 90 | } 91 | else 92 | { 93 | rightWallAvailable= rightWallAvailable - 1; 94 | } 95 | } 96 | } 97 | 98 | void cellFastForward() 99 | { 100 | leftBase = 180; 101 | rightBase = 176; 102 | leftWallAvailable = 0; 103 | rightWallAvailable = 0; 104 | frontWallAvailable = 0; 105 | encoderLeftCount = encoderLeftCount + 1273; 106 | encoderRightCount = encoderRightCount + 1273; 107 | while (leftEncoder <= encoderLeftCount || rightEncoder <= encoderRightCount) 108 | { 109 | wallFollow(); 110 | if (tof[2] <=240) 111 | { 112 | while (tof[2] > 150) 113 | { 114 | wallFollow(); 115 | } 116 | leftEncoder = encoderLeftCount + 1; 117 | rightEncoder = encoderRightCount + 1; 118 | test = 2; 119 | break; 120 | } 121 | } 122 | } 123 | 124 | 125 | void leftSmoothTurn() 126 | { 127 | leftBase = 180; 128 | rightBase = 176; 129 | encoderLeftCount = 0; 130 | encoderRightCount = 0; 131 | leftEncoder = 0; 132 | rightEncoder = 0; 133 | encoderRightCount = encoderRightCount + 300; 134 | while (rightEncoder <= encoderRightCount) 135 | { 136 | dif = rightEncoder - encoderRightCount + 300; 137 | rightBase = rightBase + int(dif * 0.1); 138 | leftBase = leftBase - int(dif/3); 139 | forwardBase(); 140 | } 141 | rightBase = 210; 142 | leftBase = 70; 143 | encoderLeftCount = 0; 144 | encoderRightCount = 0; 145 | leftEncoder = 0; 146 | rightEncoder = 0; 147 | encoderRightCount = encoderRightCount + 860; 148 | while (rightEncoder <= encoderRightCount) 149 | { 150 | //rightForwardBase(); 151 | //leftBrake(); 152 | forwardBase(); 153 | } 154 | encoderLeftCount = 0; 155 | encoderRightCount = 0; 156 | leftEncoder = 0; 157 | rightEncoder = 0; 158 | encoderRightCount = encoderRightCount + 300; 159 | while (rightEncoder <= encoderRightCount) 160 | { 161 | dif = rightEncoder - encoderRightCount + 300; 162 | rightBase = rightBase - int(dif * 0.1); 163 | leftBase = leftBase + int(dif/3); 164 | forwardBase(); 165 | } 166 | leftBase = 180; 167 | rightBase = 176; 168 | encoderRightCount = encoderRightCount + 200; 169 | while (rightEncoder <= encoderRightCount) 170 | { 171 | wallFollow(); 172 | } 173 | } 174 | 175 | 176 | void rightSmoothTurn() 177 | { 178 | leftBase = 180; 179 | rightBase = 176; 180 | encoderLeftCount = 0; 181 | encoderRightCount = 0; 182 | leftEncoder = 0; 183 | rightEncoder = 0; 184 | encoderLeftCount = encoderLeftCount + 300; 185 | while (leftEncoder <= encoderLeftCount) 186 | { 187 | dif = leftEncoder - encoderLeftCount + 300; 188 | leftBase = leftBase + int(dif * 0.1); 189 | rightBase = rightBase - int(dif/3); 190 | forwardBase(); 191 | } 192 | leftBase = 210; 193 | rightBase = 70; 194 | encoderLeftCount = 0; 195 | encoderRightCount = 0; 196 | leftEncoder = 0; 197 | rightEncoder = 0; 198 | encoderLeftCount = encoderLeftCount + 840; 199 | while (leftEncoder <= encoderLeftCount) 200 | { 201 | //leftForwardBase(); 202 | //rightBrake(); 203 | forwardBase(); 204 | } 205 | encoderLeftCount = encoderLeftCount + 300; 206 | while (leftEncoder <= encoderLeftCount) 207 | { 208 | dif = leftEncoder - encoderLeftCount + 300; 209 | leftBase = leftBase - int(dif * 0.1); 210 | rightBase = rightBase + int(dif/3); 211 | forwardBase(); 212 | } 213 | leftBase = 180; 214 | rightBase = 176; 215 | encoderLeftCount = encoderLeftCount + 200; 216 | while (leftEncoder <= encoderLeftCount) 217 | { 218 | wallFollow(); 219 | } 220 | } 221 | 222 | 223 | 224 | void rightTurn() 225 | { 226 | cellBrake(); 227 | rightAboutTurn(); 228 | cellStart(); 229 | } 230 | 231 | void leftTurn() 232 | { 233 | cellBrake(); 234 | leftAboutTurn(); 235 | cellStart(); 236 | } 237 | void cellBack() 238 | { 239 | cellBrake(); 240 | turnBack(); 241 | cellStart(); 242 | } 243 | -------------------------------------------------------------------------------- /code/piccolaMazeSolve.h: -------------------------------------------------------------------------------- 1 | void calculatePath(boolean runningNext){ 2 | floodFill2(); 3 | 4 | byte xprevious=x; 5 | byte yprevious=y; 6 | x=0; 7 | y=0; 8 | byte oldOrient= orient; 9 | orient=0; 10 | 11 | cellCount=0; 12 | 13 | while(flood2[y][x]!=1){ 14 | toMove2(); 15 | //pathQueue.enqueue(dir); 16 | 17 | 18 | if (runningNext== true){ 19 | pathQueue.enqueue(dir); 20 | } 21 | 22 | //Serial2.println(dir); 23 | 24 | if (dir=='L'){ 25 | orient = orientation(orient,'L'); 26 | } 27 | 28 | else if (dir=='R'){ 29 | orient = orientation(orient,'R'); 30 | } 31 | 32 | else if (dir=='B'){ 33 | orient = orientation(orient,'L'); 34 | orient = orientation(orient,'L'); 35 | } 36 | 37 | 38 | xprev=x; 39 | yprev=y; 40 | updateCoordinates(); 41 | 42 | 43 | } 44 | 45 | 46 | x= xprevious; 47 | y= yprevious; 48 | orient=oldOrient; 49 | buzz(); 50 | } 51 | 52 | 53 | void traverse(byte xdes, byte ydes, boolean middleSquare, boolean shortPath, boolean smooth){ 54 | 55 | byte currentx= x; 56 | byte currenty= y; 57 | 58 | if (shortPath== false){ 59 | 60 | appendDestination(xdes,ydes,middleSquare); 61 | floodFill3(); 62 | checkWallsCell(); 63 | updateWalls(x, y, orient, L, R, F); 64 | 65 | while(flood[y][x]!=0){ 66 | 67 | checkWallsCell(); 68 | updateWalls(x, y, orient, L, R, F); 69 | appendDestination(xdes,ydes,middleSquare); 70 | floodFill3(); 71 | dir= toMove(x,y,xprev,yprev,orient); 72 | 73 | if (dir=='L'){ 74 | orient = orientation(orient,'L'); 75 | if(x==0 || x== currentx){ 76 | if(y==0 || y== currenty){ 77 | leftAboutTurn(); 78 | delay(500); 79 | cellStart(); 80 | currentx=0; 81 | currenty=0; 82 | } 83 | else{ 84 | leftTurn(); 85 | } 86 | } 87 | else{ 88 | leftTurn();} 89 | } 90 | 91 | else if (dir=='R'){ 92 | orient = orientation(orient,'R'); 93 | if(x==0 || x== currentx){ 94 | if(y==0 || y== currenty){ 95 | rightAboutTurn(); 96 | delay(500); 97 | cellStart(); 98 | currentx=0; 99 | currenty=0; 100 | } 101 | else{ 102 | rightTurn(); 103 | } 104 | } 105 | else{ 106 | rightTurn();} 107 | } 108 | 109 | else if (dir=='B'){ 110 | orient = orientation(orient,'L'); 111 | orient = orientation(orient,'L'); 112 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 113 | turnBack(); 114 | delay(500); 115 | cellStart(); 116 | currentx=0; 117 | currenty=0; 118 | } 119 | else{ 120 | cellBack(); 121 | } 122 | } 123 | else{ 124 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 125 | cellStart(); 126 | currentx=0; 127 | currenty=0; 128 | } 129 | else{ 130 | cellForward(); 131 | } 132 | } 133 | xprev=x; 134 | yprev=y; 135 | updateCoordinates(); 136 | } 137 | } 138 | 139 | else{ 140 | 141 | F= false; 142 | R= false; 143 | L= false; 144 | 145 | if (smooth == true){ 146 | calculatePath(true); 147 | 148 | while (!pathQueue.isEmpty ()){ 149 | 150 | dir= pathQueue.dequeue(); 151 | 152 | if (dir=='L'){ 153 | orient = orientation(orient,'L'); 154 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 155 | leftAboutTurn(); 156 | delay(500); 157 | cellStart(); 158 | currentx=0; 159 | currenty=0; 160 | } 161 | else{ 162 | leftSmoothTurn();} 163 | 164 | } 165 | 166 | else if (dir=='R'){ 167 | orient = orientation(orient,'R'); 168 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 169 | rightAboutTurn(); 170 | delay(500); 171 | cellStart(); 172 | currentx=0; 173 | currenty=0; 174 | } 175 | else{ 176 | rightSmoothTurn();} 177 | } 178 | 179 | else if (dir=='B'){ 180 | orient = orientation(orient,'L'); 181 | orient = orientation(orient,'L'); 182 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 183 | turnBack(); 184 | delay(500); 185 | cellStart(); 186 | currentx=0; 187 | currenty=0; 188 | } 189 | else{ 190 | cellBack();} 191 | } 192 | 193 | else{ 194 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 195 | cellStart(); 196 | currentx=0; 197 | currenty=0; 198 | } 199 | else{ 200 | cellForward();} 201 | } 202 | 203 | xprev=x; 204 | yprev=y; 205 | updateCoordinates(); 206 | 207 | } 208 | 209 | 210 | } 211 | 212 | else{ 213 | 214 | calculatePath(true); 215 | 216 | while (!pathQueue.isEmpty ()){ 217 | 218 | dir= pathQueue.dequeue(); 219 | 220 | if (dir=='L'){ 221 | orient = orientation(orient,'L'); 222 | if((x==0 && y==0)){ 223 | leftAboutTurn(); 224 | delay(500); 225 | cellStart(); 226 | } 227 | else{ 228 | leftTurn();} 229 | 230 | } 231 | 232 | else if (dir=='R'){ 233 | orient = orientation(orient,'R'); 234 | if((x==0 && y==0)){ 235 | rightAboutTurn(); 236 | delay(500); 237 | cellStart(); 238 | } 239 | else{ 240 | rightTurn();} 241 | } 242 | 243 | else if (dir=='B'){ 244 | orient = orientation(orient,'L'); 245 | orient = orientation(orient,'L'); 246 | if((x==0 && y==0)){ 247 | turnBack(); 248 | delay(500); 249 | cellStart(); 250 | } 251 | else{ 252 | cellBack();} 253 | } 254 | 255 | else{ 256 | if((x==0 && y==0)){ 257 | cellStart(); 258 | } 259 | else{ 260 | cellForward();} 261 | } 262 | 263 | xprev=x; 264 | yprev=y; 265 | updateCoordinates(); 266 | 267 | } 268 | } 269 | } 270 | } 271 | 272 | 273 | void fixOrientation(){ 274 | while(orient!=0){ 275 | leftAboutTurn(); 276 | orient = orientation(orient,'L'); 277 | delay(500); 278 | } 279 | } 280 | 281 | void loadCells(){ 282 | for (int i=0;i<14;i++){ 283 | for (int j=0; j<14;j++){ 284 | cells[j][i]= EEPROM.read(i*14+j); 285 | } 286 | } 287 | } 288 | 289 | void writeCells(){ 290 | for (int i=0;i<14;i++){ 291 | for (int j=0; j<14;j++){ 292 | EEPROM.write(i*14+j ,cells[j][i]); 293 | } 294 | } 295 | } 296 | 297 | 298 | void searchStates(){ 299 | byte searchState= EEPROM.read(200); 300 | if (searchState==3){ searchState=0; } 301 | loadCells(); 302 | 303 | if (searchState>0){ 304 | if (searchState==1){ 305 | mazeStart(); 306 | if (selectMode==1){ 307 | traverse(0,13,false,false,false); 308 | cellBrake(); 309 | buzz(); 310 | traverse(0,0,true,false,false); 311 | L=0; R=0; F=0; 312 | cellBrake(); 313 | center(); 314 | traverse(0,0,false,false,false); 315 | cellBrake(); 316 | fixOrientation(); 317 | mazeStart(); 318 | 319 | if (selectMode==2){ 320 | writeCells(); 321 | EEPROM.write(200,searchState+1); 322 | } 323 | } 324 | else if(selectMode==2){ 325 | floodFill2(); 326 | traverse(0,0,true,true,false); 327 | L=0; R=0; F=0; 328 | cellBrake(); 329 | center(); 330 | buzz(); 331 | delay(500); 332 | traverse(0,13,false,false,false); 333 | cellBrake(); 334 | buzz(); 335 | traverse(0,0,false,false,false); 336 | cellBrake(); 337 | fixOrientation(); 338 | mazeStart(); 339 | 340 | if (selectMode==2){ 341 | writeCells(); 342 | EEPROM.write(200,searchState+1); 343 | } 344 | 345 | } 346 | else if(selectMode==3){ 347 | floodFill2(); 348 | traverse(0,0,true,true,true); 349 | L=0; R=0; F=0; 350 | cellBrake(); 351 | center(); 352 | buzz(); 353 | delay(500); 354 | //traverse(0,13,false,false,false); 355 | //cellBrake(); 356 | buzz(); 357 | traverse(0,0,false,false,false); 358 | cellBrake(); 359 | fixOrientation(); 360 | mazeStart(); 361 | 362 | if (selectMode==2){ 363 | writeCells(); 364 | EEPROM.write(200,searchState+1); 365 | } 366 | 367 | } 368 | } 369 | else if(searchState==2){ 370 | mazeStart(); 371 | if (selectMode==1){ 372 | traverse(13,13,false,false,false); 373 | cellBrake(); 374 | buzz(); 375 | traverse(0,0,true,false,false); 376 | L=0; R=0; F=0; 377 | cellBrake(); 378 | center(); 379 | traverse(0,0,false,false,false); 380 | cellBrake(); 381 | fixOrientation(); 382 | mazeStart(); 383 | 384 | if (selectMode==2){ 385 | writeCells(); 386 | EEPROM.write(200,searchState+1); 387 | } 388 | } 389 | else if(selectMode==2){ 390 | floodFill2(); 391 | traverse(0,0,true,true,false); 392 | L=0; R=0; F=0; 393 | cellBrake(); 394 | center(); 395 | buzz(); 396 | delay(500); 397 | traverse(13,13,false,false,false); 398 | cellBrake(); 399 | buzz(); 400 | traverse(0,0,false,false,false); 401 | cellBrake(); 402 | fixOrientation(); 403 | mazeStart(); 404 | 405 | if (selectMode==2){ 406 | writeCells(); 407 | EEPROM.write(200,searchState+1); 408 | } 409 | 410 | } 411 | else if(selectMode==3){ 412 | floodFill2(); 413 | traverse(0,0,true,true,true); 414 | L=0; R=0; F=0; 415 | cellBrake(); 416 | center(); 417 | buzz(); 418 | delay(500); 419 | traverse(13,13,false,false,false); 420 | cellBrake(); 421 | buzz(); 422 | traverse(0,0,false,false,false); 423 | cellBrake(); 424 | fixOrientation(); 425 | mazeStart(); 426 | 427 | if (selectMode==2){ 428 | writeCells(); 429 | EEPROM.write(200,searchState+1); 430 | } 431 | 432 | } 433 | 434 | } 435 | 436 | 437 | } 438 | else{ 439 | mazeStart(); 440 | traverse(0,0,true,false,false); 441 | L= false; R= false; F= false; 442 | center(); 443 | cellBrake(); 444 | buzz(); 445 | delay(3000); 446 | traverse(13,0,false,false,false); 447 | cellBrake(); 448 | buzz(); 449 | traverse(0,0,false,false,false); 450 | cellBrake(); 451 | fixOrientation(); 452 | delay(3000); 453 | mazeStart(); 454 | if (selectMode==2){ 455 | writeCells(); 456 | EEPROM.write(200,searchState+1); 457 | } 458 | } 459 | 460 | 461 | } 462 | 463 | 464 | void eepromClear(){ 465 | for (int i=0 ;i< 250; i++){ 466 | EEPROM.write(i,0); 467 | } 468 | } -------------------------------------------------------------------------------- /piccolaMain/piccolaMazeSolve.h: -------------------------------------------------------------------------------- 1 | void eepromClear(){ 2 | for (int i=0 ;i< 250; i++){ 3 | EEPROM.write(i,0); 4 | } 5 | } 6 | 7 | 8 | void calculatePath(boolean runningNext){ 9 | floodFill2(); 10 | 11 | byte xprevious=x; 12 | byte yprevious=y; 13 | x=0; 14 | y=0; 15 | byte oldOrient= orient; 16 | orient=0; 17 | 18 | cellCount=0; 19 | 20 | while(flood2[y][x]!=1){ 21 | toMove2(); 22 | //pathQueue.enqueue(dir); 23 | 24 | 25 | if (runningNext== true){ 26 | pathQueue.enqueue(dir); 27 | } 28 | 29 | //Serial2.println(dir); 30 | 31 | if (dir=='L'){ 32 | orient = orientation(orient,'L'); 33 | } 34 | 35 | else if (dir=='R'){ 36 | orient = orientation(orient,'R'); 37 | } 38 | 39 | else if (dir=='B'){ 40 | orient = orientation(orient,'L'); 41 | orient = orientation(orient,'L'); 42 | } 43 | 44 | 45 | xprev=x; 46 | yprev=y; 47 | updateCoordinates(); 48 | 49 | 50 | } 51 | 52 | 53 | x= xprevious; 54 | y= yprevious; 55 | orient=oldOrient; 56 | buzz(); 57 | } 58 | 59 | 60 | void traverse(byte xdes, byte ydes, boolean middleSquare, boolean shortPath, boolean smooth){ 61 | 62 | byte currentx= x; 63 | byte currenty= y; 64 | 65 | if (shortPath== false){ 66 | 67 | appendDestination(xdes,ydes,middleSquare); 68 | floodFill3(); 69 | checkWallsCell(); 70 | updateWalls(x, y, orient, L, R, F); 71 | 72 | while(flood[y][x]!=0){ 73 | 74 | checkWallsCell(); 75 | updateWalls(x, y, orient, L, R, F); 76 | appendDestination(xdes,ydes,middleSquare); 77 | floodFill3(); 78 | dir= toMove(x,y,xprev,yprev,orient); 79 | 80 | if (dir=='L'){ 81 | orient = orientation(orient,'L'); 82 | if(x==0 || x== currentx){ 83 | if(y==0 || y== currenty){ 84 | leftAboutTurn(); 85 | delay(500); 86 | cellStart(); 87 | currentx=0; 88 | currenty=0; 89 | } 90 | else{ 91 | leftTurn(); 92 | } 93 | } 94 | else{ 95 | leftTurn();} 96 | } 97 | 98 | else if (dir=='R'){ 99 | orient = orientation(orient,'R'); 100 | if(x==0 || x== currentx){ 101 | if(y==0 || y== currenty){ 102 | rightAboutTurn(); 103 | delay(500); 104 | cellStart(); 105 | currentx=0; 106 | currenty=0; 107 | } 108 | else{ 109 | rightTurn(); 110 | } 111 | } 112 | else{ 113 | rightTurn();} 114 | } 115 | 116 | else if (dir=='B'){ 117 | orient = orientation(orient,'L'); 118 | orient = orientation(orient,'L'); 119 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 120 | turnBack(); 121 | delay(500); 122 | cellStart(); 123 | currentx=0; 124 | currenty=0; 125 | } 126 | else{ 127 | cellBack(); 128 | } 129 | } 130 | else{ 131 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 132 | cellStart(); 133 | currentx=0; 134 | currenty=0; 135 | } 136 | else{ 137 | cellForward(); 138 | } 139 | } 140 | xprev=x; 141 | yprev=y; 142 | updateCoordinates(); 143 | } 144 | } 145 | 146 | else{ 147 | 148 | F= false; 149 | R= false; 150 | L= false; 151 | 152 | if (smooth == true){ 153 | calculatePath(true); 154 | 155 | while (!pathQueue.isEmpty ()){ 156 | 157 | dir= pathQueue.dequeue(); 158 | 159 | if (dir=='L'){ 160 | orient = orientation(orient,'L'); 161 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 162 | leftAboutTurn(); 163 | delay(500); 164 | cellStart(); 165 | currentx=0; 166 | currenty=0; 167 | } 168 | else{ 169 | leftSmoothTurn();} 170 | 171 | } 172 | 173 | else if (dir=='R'){ 174 | orient = orientation(orient,'R'); 175 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 176 | rightAboutTurn(); 177 | delay(500); 178 | cellStart(); 179 | currentx=0; 180 | currenty=0; 181 | } 182 | else{ 183 | rightSmoothTurn();} 184 | } 185 | 186 | else if (dir=='B'){ 187 | orient = orientation(orient,'L'); 188 | orient = orientation(orient,'L'); 189 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 190 | turnBack(); 191 | delay(500); 192 | cellStart(); 193 | currentx=0; 194 | currenty=0; 195 | } 196 | else{ 197 | cellBack();} 198 | } 199 | 200 | else{ 201 | if((x==0 && y==0)||(x== currentx && y== currenty)){ 202 | cellStart(); 203 | currentx=0; 204 | currenty=0; 205 | } 206 | else{ 207 | cellForward();} 208 | } 209 | 210 | xprev=x; 211 | yprev=y; 212 | updateCoordinates(); 213 | 214 | } 215 | 216 | 217 | } 218 | 219 | else{ 220 | 221 | calculatePath(true); 222 | 223 | while (!pathQueue.isEmpty ()){ 224 | checkWallsCell(); 225 | dir= pathQueue.dequeue(); 226 | 227 | if (dir=='L'){ 228 | orient = orientation(orient,'L'); 229 | if((x==0 && y==0)){ 230 | leftAboutTurn(); 231 | delay(500); 232 | cellStart(); 233 | } 234 | else{ 235 | leftTurn();} 236 | 237 | } 238 | 239 | else if (dir=='R'){ 240 | orient = orientation(orient,'R'); 241 | if((x==0 && y==0)){ 242 | rightAboutTurn(); 243 | delay(500); 244 | cellStart(); 245 | } 246 | else{ 247 | rightTurn();} 248 | } 249 | 250 | else if (dir=='B'){ 251 | orient = orientation(orient,'L'); 252 | orient = orientation(orient,'L'); 253 | if((x==0 && y==0)){ 254 | turnBack(); 255 | delay(500); 256 | cellStart(); 257 | } 258 | else{ 259 | cellBack();} 260 | } 261 | 262 | else{ 263 | if((x==0 && y==0)){ 264 | cellStart(); 265 | } 266 | else{ 267 | cellForward();} 268 | } 269 | 270 | xprev=x; 271 | yprev=y; 272 | updateCoordinates(); 273 | 274 | } 275 | } 276 | } 277 | } 278 | 279 | 280 | void fixOrientation(){ 281 | while(orient!=0){ 282 | leftAboutTurn(); 283 | orient = orientation(orient,'L'); 284 | delay(500); 285 | } 286 | } 287 | 288 | void loadCells(){ 289 | for (int i=0;i<14;i++){ 290 | for (int j=0; j<14;j++){ 291 | cells[j][i]= EEPROM.read(i*14+j); 292 | } 293 | } 294 | } 295 | 296 | void writeCells(){ 297 | for (int i=0;i<14;i++){ 298 | for (int j=0; j<14;j++){ 299 | EEPROM.write(i*14+j ,cells[j][i]); 300 | } 301 | } 302 | } 303 | 304 | 305 | void searchStates(){ 306 | byte searchState= EEPROM.read(200); 307 | 308 | if (searchState==3){ searchState=0; } 309 | loadCells(); 310 | 311 | 312 | 313 | 314 | if (searchState==0){ 315 | mazeStart(); 316 | if (selectMode==1){ 317 | traverse(0,0,true,false,false); 318 | L= false; R= false; F= false; 319 | center(); 320 | cellBrake(); 321 | buzz(); 322 | delay(3000); 323 | writeCells(); 324 | traverse(13,0,false,false,false); 325 | cellBrake(); 326 | buzz(); 327 | traverse(0,0,false,false,false); 328 | cellBrake(); 329 | buzz(); 330 | fixOrientation(); 331 | delay(3000); 332 | mazeStart(); 333 | 334 | if (selectMode==2){ 335 | writeCells(); 336 | EEPROM.write(200,searchState+1); 337 | } 338 | else if(selectMode==3){ 339 | eepromClear(); 340 | delay(1000); 341 | buzzDone(); 342 | } 343 | } 344 | 345 | else if (selectMode==2){ 346 | traverse(0,0,true,true,false); 347 | L= false; R= false; F= false; 348 | center(); 349 | cellBrake(); 350 | buzz(); 351 | delay(3000); 352 | traverse(0,0,false,false,false); 353 | cellBrake(); 354 | buzz(); 355 | fixOrientation(); 356 | delay(3000); 357 | mazeStart(); 358 | 359 | if (selectMode==2){ 360 | writeCells(); 361 | } 362 | else if(selectMode==3){ 363 | eepromClear(); 364 | delay(1000); 365 | buzzDone(); 366 | 367 | } 368 | } 369 | else if (selectMode==3){ 370 | eepromClear(); 371 | delay(1000); 372 | buzzDone(); 373 | } 374 | } 375 | 376 | 377 | 378 | 379 | 380 | 381 | else if (searchState==1){ 382 | mazeStart(); 383 | if (selectMode==1){ 384 | traverse(0,0,true,false,false); 385 | L=0; R=0; F=0; 386 | cellBrake(); 387 | center(); 388 | traverse(0,13,false,false,false); 389 | cellBrake(); 390 | buzz(); 391 | delay(500); 392 | traverse(0,0,false,false,false); 393 | cellBrake(); 394 | fixOrientation(); 395 | mazeStart(); 396 | 397 | if (selectMode==2){ 398 | writeCells(); 399 | EEPROM.write(200,searchState+1); 400 | } 401 | else if (selectMode==3){ 402 | eepromClear(); 403 | buzzDone(); 404 | } 405 | 406 | } 407 | else if(selectMode==2){ 408 | floodFill2(); 409 | traverse(0,0,true,true,false); 410 | L=0; R=0; F=0; 411 | cellBrake(); 412 | center(); 413 | buzz(); 414 | delay(500); 415 | traverse(0,0,false,false,false); 416 | cellBrake(); 417 | fixOrientation(); 418 | mazeStart(); 419 | 420 | if (selectMode==2){ 421 | writeCells(); 422 | } 423 | else if (selectMode==3){ 424 | eepromClear(); 425 | delay(1000); 426 | buzzDone(); 427 | } 428 | } 429 | else if(selectMode==3){ 430 | eepromClear(); 431 | delay(1000); 432 | buzzDone(); 433 | 434 | } 435 | } 436 | 437 | 438 | 439 | 440 | 441 | 442 | else if(searchState==2){ 443 | mazeStart(); 444 | if (selectMode==1){ 445 | traverse(0,0,true,false,false); 446 | L=0; R=0; F=0; 447 | cellBrake(); 448 | center(); 449 | buzz(); 450 | delay(500); 451 | traverse(13,13,false,false,false); 452 | cellBrake(); 453 | buzz(); 454 | delay(500); 455 | traverse(0,0,false,false,false); 456 | cellBrake(); 457 | fixOrientation(); 458 | mazeStart(); 459 | 460 | if (selectMode==2){ 461 | writeCells(); 462 | EEPROM.write(200,searchState+1); 463 | } 464 | else if (selectMode==3){ 465 | eepromClear(); 466 | buzzDone(); 467 | } 468 | } 469 | else if(selectMode==2){ 470 | floodFill2(); 471 | traverse(0,0,true,true,false); 472 | L=0; R=0; F=0; 473 | cellBrake(); 474 | center(); 475 | buzz(); 476 | delay(500); 477 | traverse(0,0,false,false,false); 478 | cellBrake(); 479 | fixOrientation(); 480 | mazeStart(); 481 | 482 | if (selectMode==2){ 483 | writeCells(); 484 | } 485 | else if (selectMode==3){ 486 | eepromClear(); 487 | delay(1000); 488 | buzzDone(); 489 | } 490 | 491 | } 492 | else if(selectMode==3){ 493 | eepromClear(); 494 | delay(1000); 495 | buzzDone(); 496 | 497 | } 498 | 499 | } 500 | 501 | 502 | 503 | 504 | 505 | } 506 | 507 | 508 | -------------------------------------------------------------------------------- /piccolaAltium/Project Logs for piccolaAltium/piccolaPCB PCB ECO 7-16-2019 8-12-45 PM.LOG: -------------------------------------------------------------------------------- 1 | Added Component: Designator=+5V(HDR1X3) 2 | Add component. Clean all parameters for all variants 3 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 4 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 5 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 6 | Added Component: Designator=+6V(HDR1X3) 7 | Add component. Clean all parameters for all variants 8 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 9 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 10 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 11 | Added Component: Designator=GND (5V)(HDR1X3) 12 | Add component. Clean all parameters for all variants 13 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 14 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 15 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 16 | Added Component: Designator=GND (6V)(HDR1X3) 17 | Add component. Clean all parameters for all variants 18 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 19 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 20 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 21 | Added Component: Designator=leftMotor(HDR1X6) 22 | Add component. Clean all parameters for all variants 23 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 24 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 25 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 26 | Added Component: Designator=motorDriver(*SHIELD_POLOLU-713) 27 | Add component. Clean all parameters for all variants 28 | Add component (AddParameter): Name = "Availability"; Value = "Unavailable"; VariantName = "[No Variations]" 29 | Add component (AddParameter): Name = "MF"; Value = "Pololu"; VariantName = "[No Variations]" 30 | Add component (AddParameter): Name = "MP"; Value = "POLOLU-713"; VariantName = "[No Variations]" 31 | Add component (AddParameter): Name = "Package"; Value = "None"; VariantName = "[No Variations]" 32 | Add component (AddParameter): Name = "Price"; Value = "None"; VariantName = "[No Variations]" 33 | Add component (AddParameter): Name = "SnapEDA_Link"; Value = "https://www.snapeda.com/parts/POLOLU-713/Pololu/view-part/?ref=snap"; VariantName = "[No Variations]" 34 | Add component (AddParameter): Name = "Value"; Value = ""; VariantName = "[No Variations]" 35 | Added Component: Designator=rightMotor(HDR1X6) 36 | Add component. Clean all parameters for all variants 37 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 38 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 39 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 40 | Added Component: Designator=stm32(BLUE_PILL_ADV) 41 | Add component. Clean all parameters for all variants 42 | Added Component: Designator=stmPins1(HDR1X20) 43 | Add component. Clean all parameters for all variants 44 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 45 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 46 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 47 | Added Component: Designator=stmPins2(HDR1X20) 48 | Add component. Clean all parameters for all variants 49 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 50 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 51 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 52 | Added Component: Designator=tof01(HDR1X7) 53 | Add component. Clean all parameters for all variants 54 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 55 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 56 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 57 | Added Component: Designator=tof02(HDR1X7) 58 | Add component. Clean all parameters for all variants 59 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 60 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 61 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 62 | Added Component: Designator=tof03(HDR1X7) 63 | Add component. Clean all parameters for all variants 64 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 65 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 66 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 67 | Added Component: Designator=tof04(HDR1X7) 68 | Add component. Clean all parameters for all variants 69 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 70 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 71 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 72 | Added Component: Designator=tof05(HDR1X7) 73 | Add component. Clean all parameters for all variants 74 | Add component (AddParameter): Name = "LatestRevisionDate"; Value = "17-Jul-2002"; VariantName = "[No Variations]" 75 | Add component (AddParameter): Name = "LatestRevisionNote"; Value = "Re-released for DXP Platform."; VariantName = "[No Variations]" 76 | Add component (AddParameter): Name = "Publisher"; Value = "Altium Limited"; VariantName = "[No Variations]" 77 | Added Pin To Net: NetName=Net+5V_1 Pin=+5V-1 78 | Added Pin To Net: NetName=Net+5V_1 Pin=+5V-2 79 | Added Pin To Net: NetName=Net+5V_1 Pin=+5V-3 80 | Added Pin To Net: NetName=Net+5V_1 Pin=motorDriver-2 81 | Added Pin To Net: NetName=Net+5V_1 Pin=stm32-P$18 82 | Added Pin To Net: NetName=Net+5V_1 Pin=stmPins2-18 83 | Added Net: Name=Net+5V_1 84 | Added Pin To Net: NetName=Net+6V_1 Pin=+6V-1 85 | Added Pin To Net: NetName=Net+6V_1 Pin=+6V-2 86 | Added Pin To Net: NetName=Net+6V_1 Pin=+6V-3 87 | Added Pin To Net: NetName=Net+6V_1 Pin=motorDriver-7 88 | Added Net: Name=Net+6V_1 89 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=GND (5V)-1 90 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=GND (5V)-2 91 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=GND (5V)-3 92 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=motorDriver-1 93 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=stm32-P$19 94 | Added Pin To Net: NetName=NetGND (5V)_1 Pin=stmPins2-19 95 | Added Net: Name=NetGND (5V)_1 96 | Added Pin To Net: NetName=NetGND (6V)_1 Pin=GND (6V)-1 97 | Added Pin To Net: NetName=NetGND (6V)_1 Pin=GND (6V)-2 98 | Added Pin To Net: NetName=NetGND (6V)_1 Pin=GND (6V)-3 99 | Added Pin To Net: NetName=NetGND (6V)_1 Pin=motorDriver-8 100 | Added Net: Name=NetGND (6V)_1 101 | Added Pin To Net: NetName=Netstm32_P$1 Pin=stm32-P$1 102 | Added Pin To Net: NetName=Netstm32_P$1 Pin=stmPins2-1 103 | Added Net: Name=Netstm32_P$1 104 | Added Pin To Net: NetName=Netstm32_P$2 Pin=stm32-P$2 105 | Added Pin To Net: NetName=Netstm32_P$2 Pin=stmPins2-2 106 | Added Net: Name=Netstm32_P$2 107 | Added Pin To Net: NetName=Netstm32_P$3 Pin=stm32-P$3 108 | Added Pin To Net: NetName=Netstm32_P$3 Pin=stmPins2-3 109 | Added Net: Name=Netstm32_P$3 110 | Added Pin To Net: NetName=Netstm32_P$4 Pin=stm32-P$4 111 | Added Pin To Net: NetName=Netstm32_P$4 Pin=stmPins2-4 112 | Added Net: Name=Netstm32_P$4 113 | Added Pin To Net: NetName=Netstm32_P$5 Pin=stm32-P$5 114 | Added Pin To Net: NetName=Netstm32_P$5 Pin=stmPins2-5 115 | Added Net: Name=Netstm32_P$5 116 | Added Pin To Net: NetName=Netstm32_P$6 Pin=stm32-P$6 117 | Added Pin To Net: NetName=Netstm32_P$6 Pin=stmPins2-6 118 | Added Net: Name=Netstm32_P$6 119 | Added Pin To Net: NetName=Netstm32_P$7 Pin=stm32-P$7 120 | Added Pin To Net: NetName=Netstm32_P$7 Pin=stmPins2-7 121 | Added Net: Name=Netstm32_P$7 122 | Added Pin To Net: NetName=Netstm32_P$8 Pin=stm32-P$8 123 | Added Pin To Net: NetName=Netstm32_P$8 Pin=stmPins2-8 124 | Added Net: Name=Netstm32_P$8 125 | Added Pin To Net: NetName=Netstm32_P$9 Pin=stm32-P$9 126 | Added Pin To Net: NetName=Netstm32_P$9 Pin=stmPins2-9 127 | Added Net: Name=Netstm32_P$9 128 | Added Pin To Net: NetName=Netstm32_P$10 Pin=stm32-P$10 129 | Added Pin To Net: NetName=Netstm32_P$10 Pin=stmPins2-10 130 | Added Net: Name=Netstm32_P$10 131 | Added Pin To Net: NetName=Netstm32_P$11 Pin=stm32-P$11 132 | Added Pin To Net: NetName=Netstm32_P$11 Pin=stmPins2-11 133 | Added Net: Name=Netstm32_P$11 134 | Added Pin To Net: NetName=Netstm32_P$12 Pin=stm32-P$12 135 | Added Pin To Net: NetName=Netstm32_P$12 Pin=stmPins2-12 136 | Added Net: Name=Netstm32_P$12 137 | Added Pin To Net: NetName=Netstm32_P$13 Pin=stm32-P$13 138 | Added Pin To Net: NetName=Netstm32_P$13 Pin=stmPins2-13 139 | Added Net: Name=Netstm32_P$13 140 | Added Pin To Net: NetName=Netstm32_P$14 Pin=stm32-P$14 141 | Added Pin To Net: NetName=Netstm32_P$14 Pin=stmPins2-14 142 | Added Net: Name=Netstm32_P$14 143 | Added Pin To Net: NetName=Netstm32_P$15 Pin=stm32-P$15 144 | Added Pin To Net: NetName=Netstm32_P$15 Pin=stmPins2-15 145 | Added Net: Name=Netstm32_P$15 146 | Added Pin To Net: NetName=Netstm32_P$16 Pin=stm32-P$16 147 | Added Pin To Net: NetName=Netstm32_P$16 Pin=stmPins2-16 148 | Added Net: Name=Netstm32_P$16 149 | Added Pin To Net: NetName=Netstm32_P$17 Pin=stm32-P$17 150 | Added Pin To Net: NetName=Netstm32_P$17 Pin=stmPins2-17 151 | Added Net: Name=Netstm32_P$17 152 | Added Pin To Net: NetName=Netstm32_P$20 Pin=stm32-P$20 153 | Added Pin To Net: NetName=Netstm32_P$20 Pin=stmPins2-20 154 | Added Net: Name=Netstm32_P$20 155 | Added Pin To Net: NetName=Netstm32_P$21 Pin=stm32-P$21 156 | Added Pin To Net: NetName=Netstm32_P$21 Pin=stmPins1-20 157 | Added Net: Name=Netstm32_P$21 158 | Added Pin To Net: NetName=Netstm32_P$22 Pin=stm32-P$22 159 | Added Pin To Net: NetName=Netstm32_P$22 Pin=stmPins1-19 160 | Added Net: Name=Netstm32_P$22 161 | Added Pin To Net: NetName=Netstm32_P$23 Pin=stm32-P$23 162 | Added Pin To Net: NetName=Netstm32_P$23 Pin=stmPins1-18 163 | Added Net: Name=Netstm32_P$23 164 | Added Pin To Net: NetName=Netstm32_P$24 Pin=stm32-P$24 165 | Added Pin To Net: NetName=Netstm32_P$24 Pin=stmPins1-17 166 | Added Net: Name=Netstm32_P$24 167 | Added Pin To Net: NetName=Netstm32_P$25 Pin=stm32-P$25 168 | Added Pin To Net: NetName=Netstm32_P$25 Pin=stmPins1-16 169 | Added Net: Name=Netstm32_P$25 170 | Added Pin To Net: NetName=Netstm32_P$26 Pin=stm32-P$26 171 | Added Pin To Net: NetName=Netstm32_P$26 Pin=stmPins1-15 172 | Added Net: Name=Netstm32_P$26 173 | Added Pin To Net: NetName=Netstm32_P$27 Pin=stm32-P$27 174 | Added Pin To Net: NetName=Netstm32_P$27 Pin=stmPins1-14 175 | Added Net: Name=Netstm32_P$27 176 | Added Pin To Net: NetName=Netstm32_P$28 Pin=stm32-P$28 177 | Added Pin To Net: NetName=Netstm32_P$28 Pin=stmPins1-13 178 | Added Net: Name=Netstm32_P$28 179 | Added Pin To Net: NetName=Netstm32_P$29 Pin=stm32-P$29 180 | Added Pin To Net: NetName=Netstm32_P$29 Pin=stmPins1-12 181 | Added Net: Name=Netstm32_P$29 182 | Added Pin To Net: NetName=Netstm32_P$30 Pin=stm32-P$30 183 | Added Pin To Net: NetName=Netstm32_P$30 Pin=stmPins1-11 184 | Added Net: Name=Netstm32_P$30 185 | Added Pin To Net: NetName=Netstm32_P$31 Pin=stm32-P$31 186 | Added Pin To Net: NetName=Netstm32_P$31 Pin=stmPins1-10 187 | Added Net: Name=Netstm32_P$31 188 | Added Pin To Net: NetName=Netstm32_P$32 Pin=stm32-P$32 189 | Added Pin To Net: NetName=Netstm32_P$32 Pin=stmPins1-9 190 | Added Net: Name=Netstm32_P$32 191 | Added Pin To Net: NetName=Netstm32_P$33 Pin=stm32-P$33 192 | Added Pin To Net: NetName=Netstm32_P$33 Pin=stmPins1-8 193 | Added Net: Name=Netstm32_P$33 194 | Added Pin To Net: NetName=Netstm32_P$34 Pin=stm32-P$34 195 | Added Pin To Net: NetName=Netstm32_P$34 Pin=stmPins1-7 196 | Added Net: Name=Netstm32_P$34 197 | Added Pin To Net: NetName=Netstm32_P$35 Pin=stm32-P$35 198 | Added Pin To Net: NetName=Netstm32_P$35 Pin=stmPins1-6 199 | Added Net: Name=Netstm32_P$35 200 | Added Pin To Net: NetName=Netstm32_P$36 Pin=stm32-P$36 201 | Added Pin To Net: NetName=Netstm32_P$36 Pin=stmPins1-5 202 | Added Net: Name=Netstm32_P$36 203 | Added Pin To Net: NetName=Netstm32_P$37 Pin=stm32-P$37 204 | Added Pin To Net: NetName=Netstm32_P$37 Pin=stmPins1-4 205 | Added Net: Name=Netstm32_P$37 206 | Added Pin To Net: NetName=Netstm32_P$38 Pin=stm32-P$38 207 | Added Pin To Net: NetName=Netstm32_P$38 Pin=stmPins1-3 208 | Added Net: Name=Netstm32_P$38 209 | Added Pin To Net: NetName=Netstm32_P$39 Pin=stm32-P$39 210 | Added Pin To Net: NetName=Netstm32_P$39 Pin=stmPins1-2 211 | Added Net: Name=Netstm32_P$39 212 | Added Pin To Net: NetName=Netstm32_P$40 Pin=stm32-P$40 213 | Added Pin To Net: NetName=Netstm32_P$40 Pin=stmPins1-1 214 | Added Net: Name=Netstm32_P$40 215 | Added Class: Name=piccaloSchematic 216 | Added Room: Name=piccaloSchematic 217 | -------------------------------------------------------------------------------- /code/piccolaZlgo.h: -------------------------------------------------------------------------------- 1 | 2 | void appendDestination(byte xdes,byte ydes, boolean middleSquare){ 3 | 4 | if (middleSquare == true){ 5 | for (int i=0; i<14; i++){ 6 | for (int j=0; j<14; j++){ 7 | flood[i][j]=254; 8 | } 9 | } 10 | 11 | flood[7][7]=0; 12 | flood[6][7]=0; 13 | flood[7][6]=0; 14 | flood[6][6]=0; 15 | 16 | queue.enqueue(7); 17 | queue.enqueue(7); 18 | queue.enqueue(6); 19 | queue.enqueue(7); 20 | queue.enqueue(7); 21 | queue.enqueue(6); 22 | queue.enqueue(6); 23 | queue.enqueue(6); 24 | } 25 | 26 | else{ 27 | for (int i=0; i<14; i++){ 28 | for (int j=0; j<14; j++){ 29 | flood[i][j]=254; 30 | } 31 | } 32 | 33 | flood[ydes][xdes]=0; 34 | 35 | queue.enqueue(ydes); 36 | queue.enqueue(xdes); 37 | } 38 | } 39 | 40 | 41 | byte orientation(byte orient, char turning){ 42 | if (turning== 'L'){ 43 | orient=orient-1; 44 | if (orient==-1|| orient== 255) 45 | orient=3; 46 | } 47 | else if(turning== 'R'){ 48 | orient=orient+1; 49 | if (orient==4|| orient==255) 50 | orient=0; 51 | } 52 | else if(turning== 'B'){ 53 | if (orient==0) 54 | orient=2; 55 | else if (orient==1) 56 | orient=3; 57 | else if (orient==2) 58 | orient=0; 59 | else if (orient==3) 60 | orient=1; 61 | } 62 | 63 | return(orient); 64 | } 65 | 66 | byte updateCoordinates(){//byte x, byte y, byte orient){ 67 | 68 | if (orient==0) 69 | y=y+1; 70 | if (orient==1) 71 | x=x+1; 72 | if (orient==2) 73 | y=y-1; 74 | if (orient==3) 75 | x=x-1; 76 | 77 | //return(x,y); 78 | } 79 | 80 | 81 | 82 | void updateWalls(byte x, byte y, byte orient, boolean L, boolean R, boolean F){ 83 | if(L && R && F){ 84 | if (orient==0) 85 | cells[y][x]= 13; 86 | else if (orient==1) 87 | cells[y][x]= 12; 88 | else if (orient==2) 89 | cells[y][x]= 11; 90 | else if (orient==3) 91 | cells[y][x]= 14; 92 | } 93 | 94 | else if (L && R && !F){ 95 | if (orient==0 || orient== 2) 96 | cells[y][x]= 9; 97 | else if (orient==1 || orient==3) 98 | cells[y][x]= 10; 99 | } 100 | 101 | else if (L && F && !R){ 102 | if (orient==0) 103 | cells[y][x]= 8; 104 | else if (orient==1) 105 | cells[y][x]= 7; 106 | else if (orient==2) 107 | cells[y][x]= 6; 108 | else if (orient==3) 109 | cells[y][x]= 5; 110 | } 111 | 112 | else if (R && F && !L){ 113 | if (orient==0) 114 | cells[y][x]= 7; 115 | else if (orient==1) 116 | cells[y][x]= 6; 117 | else if (orient==2) 118 | cells[y][x]= 5; 119 | else if (orient==3) 120 | cells[y][x]= 8; 121 | } 122 | 123 | else if(F){ 124 | if (orient==0) 125 | cells[y][x]= 2; 126 | else if (orient==1) 127 | cells[y][x]= 3; 128 | else if (orient==2) 129 | cells[y][x]= 4; 130 | else if (orient==3) 131 | cells[y][x]= 1; 132 | } 133 | 134 | else if(L){ 135 | if (orient==0) 136 | cells[y][x]= 1; 137 | else if (orient==1) 138 | cells[y][x]= 2; 139 | else if (orient==2) 140 | cells[y][x]= 3; 141 | else if (orient==3) 142 | cells[y][x]= 4; 143 | } 144 | 145 | else if(R){ 146 | if (orient==0) 147 | cells[y][x]= 3; 148 | else if (orient==1) 149 | cells[y][x]= 4; 150 | else if (orient==2) 151 | cells[y][x]= 1; 152 | else if (orient==3) 153 | cells[y][x]= 2; 154 | } 155 | 156 | else 157 | cells[y][x]= 15; 158 | 159 | 160 | } 161 | 162 | boolean isAccessible(byte x, byte y, byte xNext, byte yNext){ 163 | if (xNext==254 || yNext== 254){ 164 | return(false); 165 | } 166 | if (x==xNext){ 167 | if(y>yNext){ 168 | if(cells[y][x]==4 || cells[y][x]==5 || cells[y][x]==6 || cells[y][x]==10 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==14 || cells[yNext][xNext]==2 || cells[yNext][xNext]==7 || cells[yNext][xNext]==8 || cells[yNext][xNext]==10 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 169 | return (false); 170 | else 171 | return(true); 172 | } 173 | else{ 174 | if(cells[y][x]==2 || cells[y][x]==7 || cells[y][x]==8 || cells[y][x]==10 || cells[y][x]==12 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==4 || cells[yNext][xNext]==5 || cells[yNext][xNext]==6 || cells[yNext][xNext]==10 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==14 ) 175 | return (false); 176 | else 177 | return(true); 178 | } 179 | } 180 | else if (y==yNext){ 181 | if(x>xNext){ 182 | if(cells[y][x]==1 || cells[y][x]==5 || cells[y][x]==8 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==3 || cells[yNext][xNext]==6 || cells[yNext][xNext]==7 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 ) 183 | return (false); 184 | else 185 | return (true); 186 | } 187 | else{ 188 | if(cells[y][x]==3 || cells[y][x]==6 || cells[y][x]==7 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==13 || cells[yNext][xNext]==1 || cells[yNext][xNext]==5 || cells[yNext][xNext]==8 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 189 | return (false); 190 | else 191 | return (true); 192 | } 193 | 194 | 195 | } 196 | 197 | } 198 | 199 | byte getSurrounds(byte x,byte y){ 200 | x_3= x-1; 201 | y_3=y; 202 | x_0=x; 203 | y_0=y+1; 204 | x_1=x+1; 205 | y_1=y; 206 | x_2=x; 207 | y_2=y-1; 208 | if(x_1>= mazeSize){ 209 | x_1=254;} 210 | if(y_0>= mazeSize){ 211 | y_0=254;} 212 | if(x_3== 255){ 213 | x_3 =254;} 214 | if(y_2== 255){ 215 | y_2=254;} 216 | return (x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3); //order of cells- north,east,south,west 217 | } 218 | 219 | 220 | void floodFill2(){ 221 | 222 | for (int i=0;i<14;i++){ 223 | for (int j=0;j<14;j++){ 224 | flood2[i][j]=0; 225 | } 226 | } 227 | 228 | flood2[7][7]=1; 229 | flood2[6][7]=1; 230 | flood2[7][6]=1; 231 | flood2[6][6]=1; 232 | 233 | queue.enqueue(7); 234 | queue.enqueue(7); 235 | queue.enqueue(6); 236 | queue.enqueue(7); 237 | queue.enqueue(7); 238 | queue.enqueue(6); 239 | queue.enqueue(6); 240 | queue.enqueue(6); 241 | 242 | while (!queue.isEmpty ()){ 243 | yrun=queue.dequeue(); 244 | xrun=queue.dequeue(); 245 | 246 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3= getSurrounds(xrun,yrun); 247 | if(x_0>=0 && y_0>=0 && cells[y_0][x_0]!=0){ 248 | if (flood2[y_0][x_0]==0){ 249 | if (isAccessible(xrun,yrun,x_0,y_0)){ 250 | flood2[y_0][x_0]= flood2[yrun][xrun]+1; 251 | queue.enqueue(y_0); 252 | queue.enqueue(x_0); 253 | } 254 | } 255 | } 256 | if(x_1>=0 && y_1>=0 && cells[y_1][x_1]!=0){ 257 | if (flood2[y_1][x_1]==0){ 258 | if (isAccessible(xrun,yrun,x_1,y_1)){ 259 | flood2[y_1][x_1]= flood2[yrun][xrun]+1; 260 | queue.enqueue(y_1); 261 | queue.enqueue(x_1); 262 | } 263 | } 264 | } 265 | if(x_2>=0 && y_2>=0 && cells[y_2][x_2]!=0){ 266 | if (flood2[y_2][x_2]==0){ 267 | if (isAccessible(xrun,yrun,x_2,y_2)){ 268 | flood2[y_2][x_2]=flood2[yrun][xrun]+1; 269 | queue.enqueue(y_2); 270 | queue.enqueue(x_2); 271 | } 272 | } 273 | } 274 | if(x_3>=0 && y_3>=0 && cells[y_3][x_3]!=0){ 275 | if (flood2[y_3][x_3]==0){ 276 | if (isAccessible(xrun,yrun,x_3,y_3)){ 277 | flood2[y_3][x_3]=flood2[yrun][xrun]+1; 278 | queue.enqueue(y_3); 279 | queue.enqueue(x_3); 280 | } 281 | } 282 | } 283 | } 284 | 285 | } 286 | 287 | 288 | void floodFill3(){ 289 | 290 | while (!queue.isEmpty ()){ 291 | 292 | yrun=queue.dequeue(); 293 | xrun=queue.dequeue(); 294 | 295 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(xrun,yrun); 296 | if(x_0>=0 && y_0>=0 ){ 297 | if (flood[y_0][x_0]==254){ 298 | if (isAccessible(xrun,yrun,x_0,y_0)){ 299 | flood[y_0][x_0]=flood[yrun][xrun]+1; 300 | queue.enqueue(y_0); 301 | queue.enqueue(x_0); 302 | } 303 | } 304 | } 305 | if(x_1>=0 and y_1>=0){ 306 | if (flood[y_1][x_1]==254){ 307 | if (isAccessible(xrun,yrun,x_1,y_1)){ 308 | flood[y_1][x_1]=flood[yrun][xrun]+1; 309 | queue.enqueue(y_1); 310 | queue.enqueue(x_1); 311 | } 312 | } 313 | } 314 | if(x_2>=0 and y_2>=0 ){ 315 | if (flood[y_2][x_2]==254){ 316 | if (isAccessible(xrun,yrun,x_2,y_2)){ 317 | flood[y_2][x_2]=flood[yrun][xrun]+1; 318 | queue.enqueue(y_2); 319 | queue.enqueue(x_2); 320 | } 321 | } 322 | } 323 | if(x_3>=0 and y_3>=0 ){ 324 | if (flood[y_3][x_3]==254){ 325 | if (isAccessible(xrun,yrun,x_3,y_3)){ 326 | flood[y_3][x_3]=flood[yrun][xrun]+1; 327 | queue.enqueue(y_3); 328 | queue.enqueue(x_3); 329 | } 330 | } 331 | } 332 | } 333 | } 334 | 335 | 336 | 337 | char toMove(byte x,byte y,byte xprev,byte yprev,byte orient){ 338 | 339 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(x,y); 340 | 341 | byte val= flood[y][x]; 342 | byte prev=254; 343 | byte minVals[4]={254,254,254,254}; 344 | 345 | /*for (int i=0; i<4;i++){ 346 | Serial.print(minVals[i]); 347 | Serial.print(' '); 348 | } 349 | Serial.println(' ');*/ 350 | 351 | 352 | 353 | if (isAccessible(x,y,x_0,y_0)){ 354 | if (x_0==xprev && y_0==yprev) 355 | prev=0; 356 | minVals[0]= flood[y_0][x_0]; 357 | //Serial.println(1000); 358 | } 359 | else 360 | minVals[0]=254; 361 | 362 | if (isAccessible(x,y,x_1,y_1)){ 363 | if (x_1==xprev && y_1==yprev) 364 | prev=1; 365 | minVals[1]= flood[y_1][x_1]; 366 | //Serial.println(2000); 367 | } 368 | else 369 | minVals[1]=254; 370 | 371 | if (isAccessible(x,y,x_2,y_2)){ 372 | if (x_2==xprev && y_2==yprev) 373 | prev=2; 374 | minVals[2]= flood[y_2][x_2]; 375 | //Serial.println(3000); 376 | } 377 | else 378 | minVals[2]=254; 379 | 380 | if (isAccessible(x,y,x_3,y_3)){ 381 | if (x_3==xprev && y_3==yprev) 382 | prev=3; 383 | minVals[3]= flood[y_3][x_3]; 384 | //Serial.println(4000); 385 | } 386 | else 387 | minVals[3]=254; 388 | 389 | 390 | //Serial.println(prev); 391 | byte minVal=254; 392 | byte minCell=0; 393 | byte noMovements=0; 394 | 395 | 396 | for (int i=0; i<4;i++){ 397 | //Serial.print(minVals[i]); 398 | //Serial.print(' '); 399 | if (minVals[i]!= 254){ 400 | noMovements+=1;} 401 | } 402 | //Serial.println(' '); 403 | for (int i=0; i<4;i++){ 404 | if (minVals[i]yNext){ 168 | if(cells[y][x]==4 || cells[y][x]==5 || cells[y][x]==6 || cells[y][x]==10 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==14 || cells[yNext][xNext]==2 || cells[yNext][xNext]==7 || cells[yNext][xNext]==8 || cells[yNext][xNext]==10 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 169 | return (false); 170 | else 171 | return(true); 172 | } 173 | else{ 174 | if(cells[y][x]==2 || cells[y][x]==7 || cells[y][x]==8 || cells[y][x]==10 || cells[y][x]==12 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==4 || cells[yNext][xNext]==5 || cells[yNext][xNext]==6 || cells[yNext][xNext]==10 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==14 ) 175 | return (false); 176 | else 177 | return(true); 178 | } 179 | } 180 | else if (y==yNext){ 181 | if(x>xNext){ 182 | if(cells[y][x]==1 || cells[y][x]==5 || cells[y][x]==8 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==3 || cells[yNext][xNext]==6 || cells[yNext][xNext]==7 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 ) 183 | return (false); 184 | else 185 | return (true); 186 | } 187 | else{ 188 | if(cells[y][x]==3 || cells[y][x]==6 || cells[y][x]==7 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==13 || cells[yNext][xNext]==1 || cells[yNext][xNext]==5 || cells[yNext][xNext]==8 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 189 | return (false); 190 | else 191 | return (true); 192 | } 193 | 194 | 195 | } 196 | 197 | } 198 | 199 | byte getSurrounds(byte x,byte y){ 200 | x_3= x-1; 201 | y_3=y; 202 | x_0=x; 203 | y_0=y+1; 204 | x_1=x+1; 205 | y_1=y; 206 | x_2=x; 207 | y_2=y-1; 208 | if(x_1>= mazeSize){ 209 | x_1=254;} 210 | if(y_0>= mazeSize){ 211 | y_0=254;} 212 | if(x_3== 255){ 213 | x_3 =254;} 214 | if(y_2== 255){ 215 | y_2=254;} 216 | return (x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3); //order of cells- north,east,south,west 217 | } 218 | 219 | 220 | void floodFill2(){ 221 | 222 | for (int i=0;i<14;i++){ 223 | for (int j=0;j<14;j++){ 224 | flood2[i][j]=0; 225 | } 226 | } 227 | 228 | flood2[7][7]=1; 229 | flood2[6][7]=1; 230 | flood2[7][6]=1; 231 | flood2[6][6]=1; 232 | 233 | queue.enqueue(7); 234 | queue.enqueue(7); 235 | queue.enqueue(6); 236 | queue.enqueue(7); 237 | queue.enqueue(7); 238 | queue.enqueue(6); 239 | queue.enqueue(6); 240 | queue.enqueue(6); 241 | 242 | while (!queue.isEmpty ()){ 243 | yrun=queue.dequeue(); 244 | xrun=queue.dequeue(); 245 | 246 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3= getSurrounds(xrun,yrun); 247 | if(x_0>=0 && y_0>=0 && cells[y_0][x_0]!=0){ 248 | if (flood2[y_0][x_0]==0){ 249 | if (isAccessible(xrun,yrun,x_0,y_0)){ 250 | flood2[y_0][x_0]= flood2[yrun][xrun]+1; 251 | queue.enqueue(y_0); 252 | queue.enqueue(x_0); 253 | } 254 | } 255 | } 256 | if(x_1>=0 && y_1>=0 && cells[y_1][x_1]!=0){ 257 | if (flood2[y_1][x_1]==0){ 258 | if (isAccessible(xrun,yrun,x_1,y_1)){ 259 | flood2[y_1][x_1]= flood2[yrun][xrun]+1; 260 | queue.enqueue(y_1); 261 | queue.enqueue(x_1); 262 | } 263 | } 264 | } 265 | if(x_2>=0 && y_2>=0 && cells[y_2][x_2]!=0){ 266 | if (flood2[y_2][x_2]==0){ 267 | if (isAccessible(xrun,yrun,x_2,y_2)){ 268 | flood2[y_2][x_2]=flood2[yrun][xrun]+1; 269 | queue.enqueue(y_2); 270 | queue.enqueue(x_2); 271 | } 272 | } 273 | } 274 | if(x_3>=0 && y_3>=0 && cells[y_3][x_3]!=0){ 275 | if (flood2[y_3][x_3]==0){ 276 | if (isAccessible(xrun,yrun,x_3,y_3)){ 277 | flood2[y_3][x_3]=flood2[yrun][xrun]+1; 278 | queue.enqueue(y_3); 279 | queue.enqueue(x_3); 280 | } 281 | } 282 | } 283 | } 284 | 285 | } 286 | 287 | 288 | void floodFill3(){ 289 | 290 | while (!queue.isEmpty ()){ 291 | 292 | yrun=queue.dequeue(); 293 | xrun=queue.dequeue(); 294 | 295 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(xrun,yrun); 296 | if(x_0>=0 && y_0>=0 ){ 297 | if (flood[y_0][x_0]==254){ 298 | if (isAccessible(xrun,yrun,x_0,y_0)){ 299 | flood[y_0][x_0]=flood[yrun][xrun]+1; 300 | queue.enqueue(y_0); 301 | queue.enqueue(x_0); 302 | } 303 | } 304 | } 305 | if(x_1>=0 and y_1>=0){ 306 | if (flood[y_1][x_1]==254){ 307 | if (isAccessible(xrun,yrun,x_1,y_1)){ 308 | flood[y_1][x_1]=flood[yrun][xrun]+1; 309 | queue.enqueue(y_1); 310 | queue.enqueue(x_1); 311 | } 312 | } 313 | } 314 | if(x_2>=0 and y_2>=0 ){ 315 | if (flood[y_2][x_2]==254){ 316 | if (isAccessible(xrun,yrun,x_2,y_2)){ 317 | flood[y_2][x_2]=flood[yrun][xrun]+1; 318 | queue.enqueue(y_2); 319 | queue.enqueue(x_2); 320 | } 321 | } 322 | } 323 | if(x_3>=0 and y_3>=0 ){ 324 | if (flood[y_3][x_3]==254){ 325 | if (isAccessible(xrun,yrun,x_3,y_3)){ 326 | flood[y_3][x_3]=flood[yrun][xrun]+1; 327 | queue.enqueue(y_3); 328 | queue.enqueue(x_3); 329 | } 330 | } 331 | } 332 | } 333 | } 334 | 335 | 336 | 337 | char toMove(byte x,byte y,byte xprev,byte yprev,byte orient){ 338 | 339 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(x,y); 340 | 341 | byte val= flood[y][x]; 342 | byte prev=254; 343 | byte minVals[4]={254,254,254,254}; 344 | 345 | /*for (int i=0; i<4;i++){ 346 | Serial.print(minVals[i]); 347 | Serial.print(' '); 348 | } 349 | Serial.println(' ');*/ 350 | 351 | 352 | 353 | if (isAccessible(x,y,x_0,y_0)){ 354 | if (x_0==xprev && y_0==yprev) 355 | prev=0; 356 | minVals[0]= flood[y_0][x_0]; 357 | //Serial.println(1000); 358 | } 359 | else 360 | minVals[0]=254; 361 | 362 | if (isAccessible(x,y,x_1,y_1)){ 363 | if (x_1==xprev && y_1==yprev) 364 | prev=1; 365 | minVals[1]= flood[y_1][x_1]; 366 | //Serial.println(2000); 367 | } 368 | else 369 | minVals[1]=254; 370 | 371 | if (isAccessible(x,y,x_2,y_2)){ 372 | if (x_2==xprev && y_2==yprev) 373 | prev=2; 374 | minVals[2]= flood[y_2][x_2]; 375 | //Serial.println(3000); 376 | } 377 | else 378 | minVals[2]=254; 379 | 380 | if (isAccessible(x,y,x_3,y_3)){ 381 | if (x_3==xprev && y_3==yprev) 382 | prev=3; 383 | minVals[3]= flood[y_3][x_3]; 384 | //Serial.println(4000); 385 | } 386 | else 387 | minVals[3]=254; 388 | 389 | 390 | //Serial.println(prev); 391 | byte minVal=254; 392 | byte minCell=0; 393 | byte noMovements=0; 394 | 395 | 396 | for (int i=0; i<4;i++){ 397 | //Serial.print(minVals[i]); 398 | //Serial.print(' '); 399 | if (minVals[i]!= 254){ 400 | noMovements+=1;} 401 | } 402 | //Serial.println(' '); 403 | for (int i=0; i<4;i++){ 404 | if (minVals[i]yNext){ 168 | if(cells[y][x]==4 || cells[y][x]==5 || cells[y][x]==6 || cells[y][x]==10 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==14 || cells[yNext][xNext]==2 || cells[yNext][xNext]==7 || cells[yNext][xNext]==8 || cells[yNext][xNext]==10 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 169 | return (false); 170 | else 171 | return(true); 172 | } 173 | else{ 174 | if(cells[y][x]==2 || cells[y][x]==7 || cells[y][x]==8 || cells[y][x]==10 || cells[y][x]==12 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==4 || cells[yNext][xNext]==5 || cells[yNext][xNext]==6 || cells[yNext][xNext]==10 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==14 ) 175 | return (false); 176 | else 177 | return(true); 178 | } 179 | } 180 | else if (y==yNext){ 181 | if(x>xNext){ 182 | if(cells[y][x]==1 || cells[y][x]==5 || cells[y][x]==8 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==13 || cells[y][x]==14 || cells[yNext][xNext]==3 || cells[yNext][xNext]==6 || cells[yNext][xNext]==7 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==12 || cells[yNext][xNext]==13 ) 183 | return (false); 184 | else 185 | return (true); 186 | } 187 | else{ 188 | if(cells[y][x]==3 || cells[y][x]==6 || cells[y][x]==7 || cells[y][x]==9 || cells[y][x]==11 || cells[y][x]==12 || cells[y][x]==13 || cells[yNext][xNext]==1 || cells[yNext][xNext]==5 || cells[yNext][xNext]==8 || cells[yNext][xNext]==9 || cells[yNext][xNext]==11 || cells[yNext][xNext]==13 || cells[yNext][xNext]==14 ) 189 | return (false); 190 | else 191 | return (true); 192 | } 193 | 194 | 195 | } 196 | 197 | } 198 | 199 | byte getSurrounds(byte x,byte y){ 200 | x_3= x-1; 201 | y_3=y; 202 | x_0=x; 203 | y_0=y+1; 204 | x_1=x+1; 205 | y_1=y; 206 | x_2=x; 207 | y_2=y-1; 208 | if(x_1>= mazeSize){ 209 | x_1=254;} 210 | if(y_0>= mazeSize){ 211 | y_0=254;} 212 | if(x_3== 255){ 213 | x_3 =254;} 214 | if(y_2== 255){ 215 | y_2=254;} 216 | return (x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3); //order of cells- north,east,south,west 217 | } 218 | 219 | 220 | void floodFill2(){ 221 | 222 | for (int i=0;i<14;i++){ 223 | for (int j=0;j<14;j++){ 224 | flood2[i][j]=0; 225 | } 226 | } 227 | 228 | flood2[7][7]=1; 229 | flood2[6][7]=1; 230 | flood2[7][6]=1; 231 | flood2[6][6]=1; 232 | 233 | queue.enqueue(7); 234 | queue.enqueue(7); 235 | queue.enqueue(6); 236 | queue.enqueue(7); 237 | queue.enqueue(7); 238 | queue.enqueue(6); 239 | queue.enqueue(6); 240 | queue.enqueue(6); 241 | 242 | while (!queue.isEmpty ()){ 243 | yrun=queue.dequeue(); 244 | xrun=queue.dequeue(); 245 | 246 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3= getSurrounds(xrun,yrun); 247 | if(x_0>=0 && y_0>=0 && cells[y_0][x_0]!=0){ 248 | if (flood2[y_0][x_0]==0){ 249 | if (isAccessible(xrun,yrun,x_0,y_0)){ 250 | flood2[y_0][x_0]= flood2[yrun][xrun]+1; 251 | queue.enqueue(y_0); 252 | queue.enqueue(x_0); 253 | } 254 | } 255 | } 256 | if(x_1>=0 && y_1>=0 && cells[y_1][x_1]!=0){ 257 | if (flood2[y_1][x_1]==0){ 258 | if (isAccessible(xrun,yrun,x_1,y_1)){ 259 | flood2[y_1][x_1]= flood2[yrun][xrun]+1; 260 | queue.enqueue(y_1); 261 | queue.enqueue(x_1); 262 | } 263 | } 264 | } 265 | if(x_2>=0 && y_2>=0 && cells[y_2][x_2]!=0){ 266 | if (flood2[y_2][x_2]==0){ 267 | if (isAccessible(xrun,yrun,x_2,y_2)){ 268 | flood2[y_2][x_2]=flood2[yrun][xrun]+1; 269 | queue.enqueue(y_2); 270 | queue.enqueue(x_2); 271 | } 272 | } 273 | } 274 | if(x_3>=0 && y_3>=0 && cells[y_3][x_3]!=0){ 275 | if (flood2[y_3][x_3]==0){ 276 | if (isAccessible(xrun,yrun,x_3,y_3)){ 277 | flood2[y_3][x_3]=flood2[yrun][xrun]+1; 278 | queue.enqueue(y_3); 279 | queue.enqueue(x_3); 280 | } 281 | } 282 | } 283 | } 284 | 285 | } 286 | 287 | 288 | void floodFill3(){ 289 | 290 | while (!queue.isEmpty ()){ 291 | 292 | yrun=queue.dequeue(); 293 | xrun=queue.dequeue(); 294 | 295 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(xrun,yrun); 296 | if(x_0>=0 && y_0>=0 ){ 297 | if (flood[y_0][x_0]==254){ 298 | if (isAccessible(xrun,yrun,x_0,y_0)){ 299 | flood[y_0][x_0]=flood[yrun][xrun]+1; 300 | queue.enqueue(y_0); 301 | queue.enqueue(x_0); 302 | } 303 | } 304 | } 305 | if(x_1>=0 and y_1>=0){ 306 | if (flood[y_1][x_1]==254){ 307 | if (isAccessible(xrun,yrun,x_1,y_1)){ 308 | flood[y_1][x_1]=flood[yrun][xrun]+1; 309 | queue.enqueue(y_1); 310 | queue.enqueue(x_1); 311 | } 312 | } 313 | } 314 | if(x_2>=0 and y_2>=0 ){ 315 | if (flood[y_2][x_2]==254){ 316 | if (isAccessible(xrun,yrun,x_2,y_2)){ 317 | flood[y_2][x_2]=flood[yrun][xrun]+1; 318 | queue.enqueue(y_2); 319 | queue.enqueue(x_2); 320 | } 321 | } 322 | } 323 | if(x_3>=0 and y_3>=0 ){ 324 | if (flood[y_3][x_3]==254){ 325 | if (isAccessible(xrun,yrun,x_3,y_3)){ 326 | flood[y_3][x_3]=flood[yrun][xrun]+1; 327 | queue.enqueue(y_3); 328 | queue.enqueue(x_3); 329 | } 330 | } 331 | } 332 | } 333 | } 334 | 335 | 336 | 337 | char toMove(byte x,byte y,byte xprev,byte yprev,byte orient){ 338 | 339 | x_0,y_0,x_1,y_1,x_2,y_2,x_3,y_3 = getSurrounds(x,y); 340 | 341 | byte val= flood[y][x]; 342 | byte prev=254; 343 | byte minVals[4]={254,254,254,254}; 344 | byte visited[4]={2,2,2,2}; 345 | 346 | 347 | 348 | if (isAccessible(x,y,x_0,y_0)){ 349 | if (x_0==xprev && y_0==yprev) 350 | prev=0; 351 | if (cells[y_0][x_0]!=0) 352 | visited[0]= 1; 353 | else 354 | visited[0]= 2; 355 | minVals[0]= flood[y_0][x_0]; 356 | } 357 | else 358 | minVals[0]=254; 359 | 360 | if (isAccessible(x,y,x_1,y_1)){ 361 | if (x_1==xprev && y_1==yprev) 362 | prev=1; 363 | if (cells[y_1][x_1]!=0) 364 | visited[1]= 1; 365 | else 366 | visited[1]= 2; 367 | 368 | minVals[1]= flood[y_1][x_1]; 369 | } 370 | else 371 | minVals[1]=254; 372 | 373 | if (isAccessible(x,y,x_2,y_2)){ 374 | if (x_2==xprev && y_2==yprev) 375 | prev=2; 376 | if (cells[y_2][x_2]!=0) 377 | visited[2]= 1; 378 | else 379 | visited[2]= 2; 380 | minVals[2]= flood[y_2][x_2]; 381 | } 382 | else 383 | minVals[2]=254; 384 | 385 | if (isAccessible(x,y,x_3,y_3)){ 386 | if (x_3==xprev && y_3==yprev) 387 | prev=3; 388 | if (cells[y_3][x_3]!=0) 389 | visited[3]= 1; 390 | else 391 | visited[3]= 2; 392 | minVals[3]= flood[y_3][x_3]; 393 | } 394 | else 395 | minVals[3]=254; 396 | 397 | 398 | byte minVal=254; 399 | byte minCell=0; 400 | byte noMovements=0; 401 | 402 | 403 | for (int i=0; i<4;i++){ 404 | if (minVals[i]!= 254){ 405 | noMovements+=1;} 406 | } 407 | 408 | for (int i=0; i<4;i++){ 409 | if (minVals[i]