├── Mini_ExcavatorBluePad32_Code.ino └── Mini_Excavator_Code.ino /Mini_ExcavatorBluePad32_Code.ino: -------------------------------------------------------------------------------- 1 | #include // by Kevin Harrington 2 | #include 3 | #include "Adafruit_MCP23X17.h" 4 | 5 | // defines 6 | #define clawServoPin 5 7 | #define auxServoPin 18 8 | #define cabLights 32 9 | #define auxLights 33 10 | 11 | #define pivot0 15 12 | #define pivot1 14 13 | #define mainBoom0 9 14 | #define mainBoom1 8 15 | #define dipper0 0 16 | #define dipper1 1 17 | #define tiltAttach0 3 18 | #define tiltAttach1 2 19 | #define thumb0 11 20 | #define thumb1 10 21 | #define auxAttach0 12 22 | #define auxAttach1 13 23 | 24 | #define leftMotor0 7 25 | #define leftMotor1 6 26 | #define rightMotor0 4 27 | #define rightMotor1 5 28 | 29 | ControllerPtr myControllers[BP32_MAX_GAMEPADS]; 30 | Adafruit_MCP23X17 mcp; 31 | Servo clawServo; 32 | Servo auxServo; 33 | 34 | int dly = 250; 35 | int clawServoValue = 90; 36 | int auxServoValue = 90; 37 | int servoDelay = 0; 38 | int lightSwitchTime = 0; 39 | 40 | bool cabLightsOn = false; 41 | bool auxLightsOn = false; 42 | bool moveClawServoUp = false; 43 | bool moveClawServoDown = false; 44 | bool moveAuxServoUp = false; 45 | bool moveAuxServoDown = false; 46 | 47 | void onConnectedController(ControllerPtr ctl) { 48 | bool foundEmptySlot = false; 49 | for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { 50 | if (myControllers[i] == nullptr) { 51 | Serial.printf("CALLBACK: Controller is connected, index=%d\n", i); 52 | // Additionally, you can get certain gamepad properties like: 53 | // Model, VID, PID, BTAddr, flags, etc. 54 | ControllerProperties properties = ctl->getProperties(); 55 | Serial.printf("Controller model: %s, VID=0x%04x, PID=0x%04x\n", ctl->getModelName().c_str(), properties.vendor_id, 56 | properties.product_id); 57 | myControllers[i] = ctl; 58 | foundEmptySlot = true; 59 | break; 60 | } 61 | } 62 | if (!foundEmptySlot) { 63 | Serial.println("CALLBACK: Controller connected, but could not find empty slot"); 64 | } 65 | } 66 | void onDisconnectedController(ControllerPtr ctl) { 67 | bool foundController = false; 68 | 69 | for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { 70 | if (myControllers[i] == ctl) { 71 | Serial.printf("CALLBACK: Controller disconnected from index=%d\n", i); 72 | myControllers[i] = nullptr; 73 | foundController = true; 74 | break; 75 | } 76 | } 77 | 78 | if (!foundController) { 79 | Serial.println("CALLBACK: Controller disconnected, but not found in myControllers"); 80 | } 81 | } 82 | void processControllers() { 83 | for (auto myController : myControllers) { 84 | if (myController && myController->isConnected() && myController->hasData()) { 85 | if (myController->isGamepad()) { 86 | processGamepad(myController); 87 | } else { 88 | Serial.println("Unsupported controller"); 89 | } 90 | } 91 | } 92 | } 93 | void processGamepad(ControllerPtr ctl) { 94 | //Boom 95 | processBoom(ctl->axisY()); 96 | //Pivot 97 | processPivot(ctl->axisX()); 98 | //Dipper 99 | processDipper(ctl->axisRY()); 100 | //TiltAttach 101 | processBucket(ctl->axisRX()); 102 | //Aux 103 | processAux(ctl->dpad()); 104 | //Lights 105 | if ((millis() - lightSwitchTime) > 200) { 106 | if (ctl->thumbR() == 1) { 107 | if (!cabLightsOn) { 108 | digitalWrite(cabLights, HIGH); 109 | cabLightsOn = true; 110 | } else { 111 | digitalWrite(cabLights, LOW); 112 | cabLightsOn = false; 113 | } 114 | delay(80); 115 | } 116 | if (ctl->thumbL() == 1) { 117 | if (!auxLightsOn) { 118 | digitalWrite(auxLights, HIGH); 119 | auxLightsOn = true; 120 | } else { 121 | digitalWrite(auxLights, LOW); 122 | auxLightsOn = false; 123 | } 124 | delay(80); 125 | } 126 | lightSwitchTime = millis(); 127 | } 128 | if (ctl->r1() == 1) { 129 | mcp.digitalWrite(rightMotor0, HIGH); 130 | mcp.digitalWrite(rightMotor1, LOW); 131 | } else if (ctl->r2() == 1) { 132 | mcp.digitalWrite(rightMotor0, LOW); 133 | mcp.digitalWrite(rightMotor1, HIGH); 134 | } else if (ctl->r1() == 0 || ctl->r2() == 0) { 135 | mcp.digitalWrite(rightMotor0, LOW); 136 | mcp.digitalWrite(rightMotor1, LOW); 137 | } 138 | if (ctl->l1() == 1) { 139 | mcp.digitalWrite(leftMotor0, HIGH); 140 | mcp.digitalWrite(leftMotor1, LOW); 141 | } else if (ctl->l2() == 1) { 142 | mcp.digitalWrite(leftMotor0, LOW); 143 | mcp.digitalWrite(leftMotor1, HIGH); 144 | } else if (ctl->l1() == 0 || ctl->r2() == 0) { 145 | mcp.digitalWrite(leftMotor0, LOW); 146 | mcp.digitalWrite(leftMotor1, LOW); 147 | } 148 | 149 | if (ctl->a() == 1) { 150 | moveClawServoDown = true; 151 | } else if (ctl->y() == 1) { 152 | moveClawServoUp = true; 153 | } else { 154 | moveClawServoDown = false; 155 | moveClawServoUp = false; 156 | } 157 | if (ctl->x() == 1) { 158 | moveAuxServoDown = true; 159 | } else if (ctl->b() == 1) { 160 | moveAuxServoUp = true; 161 | } else { 162 | moveAuxServoDown = false; 163 | moveAuxServoUp = false; 164 | } 165 | if (moveClawServoUp) { 166 | if (servoDelay == 2) { 167 | if (clawServoValue >= 10 && clawServoValue < 170) { 168 | //if using a ps3 controller that was flashed an xbox360 controller change the value "2" below to a 3-4 to make up for the slower movement. 169 | clawServoValue = clawServoValue + 2; 170 | clawServo.write(clawServoValue); 171 | } 172 | servoDelay = 0; 173 | } 174 | servoDelay++; 175 | } 176 | if (moveClawServoDown) { 177 | if (servoDelay == 2) { 178 | if (clawServoValue <= 170 && clawServoValue > 10) { 179 | //if using a ps3 controller that was flashed an xbox360 controller change the value "2" below to a 3-4 to make up for the slower movement. 180 | clawServoValue = clawServoValue - 2; 181 | clawServo.write(clawServoValue); 182 | } 183 | servoDelay = 0; 184 | } 185 | servoDelay++; 186 | } 187 | if (moveAuxServoUp) { 188 | if (servoDelay == 2) { 189 | if (auxServoValue >= 10 && auxServoValue < 170) { 190 | //if using a ps3 controller that was flashed an xbox360 controller change the value "2" below to a 3-4 to make up for the slower movement. 191 | auxServoValue = auxServoValue + 2; 192 | auxServo.write(auxServoValue); 193 | } 194 | servoDelay = 0; 195 | } 196 | servoDelay++; 197 | } 198 | if (moveAuxServoDown) { 199 | if (servoDelay == 2) { 200 | if (auxServoValue <= 170 && auxServoValue > 10) { 201 | //if using a ps3 controller that was flashed an xbox360 controller change the value "2" below to a 3-4 to make up for the slower movement. 202 | auxServoValue = auxServoValue - 2; 203 | auxServo.write(auxServoValue); 204 | } 205 | servoDelay = 0; 206 | } 207 | servoDelay++; 208 | } 209 | } 210 | void processBoom(int axisYValue) { 211 | int adjustedValue = axisYValue / 2; 212 | if (adjustedValue > 100) { 213 | mcp.digitalWrite(mainBoom0, HIGH); 214 | mcp.digitalWrite(mainBoom1, LOW); 215 | } else if (adjustedValue < -100) { 216 | mcp.digitalWrite(mainBoom0, LOW); 217 | mcp.digitalWrite(mainBoom1, HIGH); 218 | } else { 219 | mcp.digitalWrite(mainBoom0, LOW); 220 | mcp.digitalWrite(mainBoom1, LOW); 221 | } 222 | } 223 | void processPivot(int axisYValue) { 224 | int adjustedValue = axisYValue / 2; 225 | if (adjustedValue > 100) { 226 | mcp.digitalWrite(pivot0, HIGH); 227 | mcp.digitalWrite(pivot1, LOW); 228 | } else if (adjustedValue < -100) { 229 | mcp.digitalWrite(pivot0, LOW); 230 | mcp.digitalWrite(pivot1, HIGH); 231 | } else { 232 | mcp.digitalWrite(pivot0, LOW); 233 | mcp.digitalWrite(pivot1, LOW); 234 | } 235 | } 236 | void processDipper(int axisYValue) { 237 | int adjustedValue = axisYValue / 2; 238 | if (adjustedValue > 100) { 239 | mcp.digitalWrite(dipper0, HIGH); 240 | mcp.digitalWrite(dipper1, LOW); 241 | } else if (adjustedValue < -100) { 242 | mcp.digitalWrite(dipper0, LOW); 243 | mcp.digitalWrite(dipper1, HIGH); 244 | } else { 245 | mcp.digitalWrite(dipper0, LOW); 246 | mcp.digitalWrite(dipper1, LOW); 247 | } 248 | } 249 | void processBucket(int axisYValue) { 250 | int adjustedValue = axisYValue / 2; 251 | if (adjustedValue > 100) { 252 | mcp.digitalWrite(tiltAttach0, HIGH); 253 | mcp.digitalWrite(tiltAttach1, LOW); 254 | } else if (adjustedValue < -100) { 255 | mcp.digitalWrite(tiltAttach0, LOW); 256 | mcp.digitalWrite(tiltAttach1, HIGH); 257 | } else { 258 | mcp.digitalWrite(tiltAttach0, LOW); 259 | mcp.digitalWrite(tiltAttach1, LOW); 260 | } 261 | } 262 | void processAux(int dpadValue) { 263 | if (dpadValue == 1) { 264 | mcp.digitalWrite(thumb0, HIGH); 265 | mcp.digitalWrite(thumb1, LOW); 266 | } else if (dpadValue == 2) { 267 | mcp.digitalWrite(thumb0, LOW); 268 | mcp.digitalWrite(thumb1, HIGH); 269 | } else { 270 | mcp.digitalWrite(thumb0, LOW); 271 | mcp.digitalWrite(thumb1, LOW); 272 | } 273 | if (dpadValue == 4) { 274 | mcp.digitalWrite(auxAttach0, HIGH); 275 | mcp.digitalWrite(auxAttach1, LOW); 276 | } else if (dpadValue == 8) { 277 | mcp.digitalWrite(auxAttach0, LOW); 278 | mcp.digitalWrite(auxAttach1, HIGH); 279 | } else { 280 | mcp.digitalWrite(auxAttach0, LOW); 281 | mcp.digitalWrite(auxAttach1, LOW); 282 | } 283 | } 284 | void setup() { 285 | 286 | Serial.begin(115200); 287 | 288 | mcp.begin_I2C(); 289 | // put your setup code here, to run once: 290 | 291 | 292 | for (int i = 0; i <= 15; i++) { 293 | mcp.pinMode(i, OUTPUT); 294 | } 295 | 296 | pinMode(clawServoPin, OUTPUT); 297 | pinMode(auxServoPin, OUTPUT); 298 | 299 | pinMode(cabLights, OUTPUT); 300 | pinMode(auxLights, OUTPUT); 301 | 302 | clawServo.attach(clawServoPin); 303 | auxServo.attach(auxServoPin); 304 | clawServo.write(clawServoValue); 305 | auxServo.write(auxServoValue); 306 | Serial.begin(115200); 307 | // put your setup code here, to run once: 308 | Serial.printf("Firmware: %s\n", BP32.firmwareVersion()); 309 | const uint8_t *addr = BP32.localBdAddress(); 310 | Serial.printf("BD Addr: %2X:%2X:%2X:%2X:%2X:%2X\n", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 311 | 312 | // Setup the Bluepad32 callbacks 313 | BP32.setup(&onConnectedController, &onDisconnectedController); 314 | 315 | // "forgetBluetoothKeys()" should be called when the user performs 316 | // a "device factory reset", or similar. 317 | // Calling "forgetBluetoothKeys" in setup() just as an example. 318 | // Forgetting Bluetooth keys prevents "paired" gamepads to reconnect. 319 | // But it might also fix some connection / re-connection issues. 320 | BP32.forgetBluetoothKeys(); 321 | 322 | // Enables mouse / touchpad support for gamepads that support them. 323 | // When enabled, controllers like DualSense and DualShock4 generate two connected devices: 324 | // - First one: the gamepad 325 | // - Second one, which is a "virtual device", is a mouse. 326 | // By default, it is disabled. 327 | BP32.enableVirtualDevice(false); 328 | // You could add additional error handling here, 329 | // such as logging the error or attempting to recover. 330 | // For example, you might attempt to reset the MCP23X17 331 | // and retry initialization before giving up completely. 332 | // Then, you could gracefully exit the program or continue 333 | // running with limited functionality. 334 | } 335 | 336 | 337 | 338 | void loop() { 339 | // This call fetches all the controllers' data. 340 | // Call this function in your main loop. 341 | bool dataUpdated = BP32.update(); 342 | if (dataUpdated) { 343 | processControllers(); 344 | } 345 | // The main loop must have some kind of "yield to lower priority task" event. 346 | // Otherwise, the watchdog will get triggered. 347 | // If your main loop doesn't have one, just add a simple `vTaskDelay(1)`. 348 | // Detailed info here: 349 | // https://stackoverflow.com/questions/66278271/task-watchdog-got-triggered-the-tasks-did-not-reset-the-watchdog-in-time 350 | 351 | // vTaskDelay(1); 352 | else { vTaskDelay(1); } 353 | } 354 | -------------------------------------------------------------------------------- /Mini_Excavator_Code.ino: -------------------------------------------------------------------------------- 1 | #include "Adafruit_MCP23X17.h" 2 | #include 3 | #include // by Kevin Harrington 4 | 5 | // defines 6 | #define clawServoPin 5 7 | #define auxServoPin 18 8 | #define cabLights 32 9 | #define auxLights 33 10 | 11 | #define pivot0 15 12 | #define pivot1 14 13 | #define mainBoom0 9 14 | #define mainBoom1 8 15 | #define secondBoom0 0 16 | #define secondBoom1 1 17 | #define tiltAttach0 3 18 | #define tiltAttach1 2 19 | #define thumb0 11 20 | #define thumb1 10 21 | #define auxAttach0 12 22 | #define auxAttach1 13 23 | 24 | #define leftMotor0 7 25 | #define leftMotor1 6 26 | #define rightMotor0 4 27 | #define rightMotor1 5 28 | 29 | 30 | Adafruit_MCP23X17 mcp; 31 | Servo clawServo; 32 | Servo auxServo; 33 | int dly = 250; 34 | int clawServoValue = 90; 35 | int auxServoValue = 90; 36 | int player = 0; 37 | int battery = 0; 38 | int servoDelay = 0; 39 | 40 | bool cabLightsOn = false; 41 | bool auxLightsOn = false; 42 | bool moveClawServoUp = false; 43 | bool moveClawServoDown = false; 44 | bool moveAuxServoUp = false; 45 | bool moveAuxServoDown = false; 46 | 47 | void notify() { 48 | //--------------- Digital D-pad button events -------------- 49 | if (Ps3.event.button_down.up) { 50 | 51 | mcp.digitalWrite(thumb0, HIGH); 52 | mcp.digitalWrite(thumb1, LOW); 53 | Serial.println("Started pressing the up button"); 54 | } 55 | if (Ps3.event.button_up.up) { 56 | 57 | mcp.digitalWrite(thumb0, LOW); 58 | mcp.digitalWrite(thumb1, LOW); 59 | Serial.println("Released the up button"); 60 | } 61 | if (Ps3.event.button_down.down) { 62 | Serial.println("Started pressing the down button"); 63 | mcp.digitalWrite(thumb0, LOW); 64 | mcp.digitalWrite(thumb1, HIGH); 65 | } 66 | if (Ps3.event.button_up.down) { 67 | Serial.println("Released the down button"); 68 | mcp.digitalWrite(thumb0, LOW); 69 | mcp.digitalWrite(thumb1, LOW); 70 | } 71 | if (Ps3.event.button_down.right) { 72 | mcp.digitalWrite(auxAttach0, HIGH); 73 | mcp.digitalWrite(auxAttach1, LOW); 74 | Serial.println("Started pressing the right button"); 75 | } 76 | if (Ps3.event.button_up.right) { 77 | mcp.digitalWrite(auxAttach0, LOW); 78 | mcp.digitalWrite(auxAttach1, LOW); 79 | Serial.println("Released the right button"); 80 | } 81 | if (Ps3.event.button_down.left) { 82 | mcp.digitalWrite(auxAttach0, LOW); 83 | mcp.digitalWrite(auxAttach1, HIGH); 84 | Serial.println("Started pressing the left button"); 85 | } 86 | if (Ps3.event.button_up.left) { 87 | mcp.digitalWrite(auxAttach0, LOW); 88 | mcp.digitalWrite(auxAttach1, LOW); 89 | Serial.println("Released the left button"); 90 | } 91 | //---------------- Analog stick value events --------------- 92 | if (abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2) { 93 | Serial.print("Moved the left stick:"); 94 | Serial.print(" x="); 95 | Serial.print(Ps3.data.analog.stick.lx, DEC); 96 | Serial.print(" y="); 97 | Serial.print(Ps3.data.analog.stick.ly, DEC); 98 | Serial.println(); 99 | int LXValue = Ps3.data.analog.stick.lx; 100 | Serial.print("LXValue ="); 101 | Serial.print(LXValue); 102 | if (LXValue > 115) { 103 | mcp.digitalWrite(pivot0, HIGH); 104 | mcp.digitalWrite(pivot1, LOW); 105 | delay(10); 106 | Serial.print("Made to into Positive"); 107 | } 108 | if (LXValue < -115) { 109 | mcp.digitalWrite(pivot0, LOW); 110 | mcp.digitalWrite(pivot1, HIGH); 111 | delay(10); 112 | Serial.print("Made to into negative"); 113 | } 114 | 115 | int LYValue = Ps3.data.analog.stick.ly; 116 | if (LYValue > 115) { 117 | mcp.digitalWrite(secondBoom0, HIGH); 118 | mcp.digitalWrite(secondBoom1, LOW); 119 | delay(10); 120 | } 121 | if (LYValue < -115) { 122 | mcp.digitalWrite(secondBoom0, LOW); 123 | mcp.digitalWrite(secondBoom1, HIGH); 124 | delay(10); 125 | } 126 | if (LXValue > -30 && LXValue < 30) { 127 | mcp.digitalWrite(pivot0, LOW); 128 | mcp.digitalWrite(pivot1, LOW); 129 | } 130 | if (LYValue > -30 && LYValue < 30) { 131 | mcp.digitalWrite(secondBoom0, LOW); 132 | mcp.digitalWrite(secondBoom1, LOW); 133 | } 134 | } 135 | 136 | if (abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2) { 137 | Serial.print("Moved the right stick:"); 138 | Serial.print(" x="); 139 | Serial.print(Ps3.data.analog.stick.rx, DEC); 140 | Serial.print(" y="); 141 | Serial.print(Ps3.data.analog.stick.ry, DEC); 142 | Serial.println(); 143 | int RXValue = (Ps3.data.analog.stick.rx); 144 | if (RXValue > 115) { 145 | mcp.digitalWrite(tiltAttach0, HIGH); 146 | mcp.digitalWrite(tiltAttach1, LOW); 147 | delay(10); 148 | Serial.print("Made to into Positive"); 149 | } 150 | if (RXValue < -115) { 151 | mcp.digitalWrite(tiltAttach0, LOW); 152 | mcp.digitalWrite(tiltAttach1, HIGH); 153 | delay(10); 154 | Serial.print("Made to into negative"); 155 | } 156 | 157 | int RYValue = (Ps3.data.analog.stick.ry); 158 | if (RYValue > 115) { 159 | mcp.digitalWrite(mainBoom0, HIGH); 160 | mcp.digitalWrite(mainBoom1, LOW); 161 | delay(10); 162 | } 163 | if (RYValue < -115) { 164 | mcp.digitalWrite(mainBoom0, LOW); 165 | mcp.digitalWrite(mainBoom1, HIGH); 166 | delay(10); 167 | } 168 | if (RXValue > -30 && RXValue < 30) { 169 | mcp.digitalWrite(tiltAttach0, LOW); 170 | mcp.digitalWrite(tiltAttach1, LOW); 171 | } 172 | if (RYValue > -30 && RYValue < 30) { 173 | mcp.digitalWrite(mainBoom0, LOW); 174 | mcp.digitalWrite(mainBoom1, LOW); 175 | } 176 | } 177 | //------------- Digital shoulder button events ------------- 178 | if (Ps3.event.button_down.l1) { 179 | mcp.digitalWrite(leftMotor0, HIGH); 180 | mcp.digitalWrite(leftMotor1, LOW); 181 | delay(10); 182 | Serial.println("Started pressing the left shoulder button"); 183 | } 184 | if (Ps3.event.button_up.l1) { 185 | mcp.digitalWrite(leftMotor0, LOW); 186 | mcp.digitalWrite(leftMotor1, LOW); 187 | delay(10); 188 | Serial.println("Released the left shoulder button"); 189 | } 190 | if (Ps3.event.button_down.r1) { 191 | mcp.digitalWrite(rightMotor0, HIGH); 192 | mcp.digitalWrite(rightMotor1, LOW); 193 | delay(10); 194 | Serial.println("Started pressing the right shoulder button"); 195 | } 196 | if (Ps3.event.button_up.r1) { 197 | mcp.digitalWrite(rightMotor0, LOW); 198 | mcp.digitalWrite(rightMotor1, LOW); 199 | delay(10); 200 | Serial.println("Released the right shoulder button"); 201 | } 202 | //-------------- Digital trigger button events ------------- 203 | if (Ps3.event.button_down.l2) { 204 | mcp.digitalWrite(leftMotor0, LOW); 205 | mcp.digitalWrite(leftMotor1, HIGH); 206 | delay(10); 207 | Serial.println("Started pressing the left trigger button"); 208 | } 209 | if (Ps3.event.button_up.l2) { 210 | mcp.digitalWrite(leftMotor0, LOW); 211 | mcp.digitalWrite(leftMotor1, LOW); 212 | delay(10); 213 | Serial.println("Released the left trigger button"); 214 | } 215 | if (Ps3.event.button_down.r2) { 216 | mcp.digitalWrite(rightMotor0, LOW); 217 | mcp.digitalWrite(rightMotor1, HIGH); 218 | delay(10); 219 | Serial.println("Started pressing the right trigger button"); 220 | } 221 | if (Ps3.event.button_up.r2) { 222 | mcp.digitalWrite(rightMotor0, LOW); 223 | mcp.digitalWrite(rightMotor1, LOW); 224 | delay(10); 225 | Serial.println("Released the right trigger button"); 226 | } 227 | //--- Digital cross/square/triangle/circle button events --- 228 | if (Ps3.event.button_down.cross) { 229 | moveClawServoUp = true; 230 | Serial.println("Started pressing the cross button"); 231 | } 232 | if (Ps3.event.button_up.cross) { 233 | moveClawServoUp = false; 234 | Serial.println("Released the cross button"); 235 | } 236 | if (Ps3.event.button_down.square) { 237 | moveAuxServoUp = true; 238 | Serial.println("Started pressing the square button"); 239 | } 240 | if (Ps3.event.button_up.square) { 241 | moveAuxServoUp = false; 242 | Serial.println("Released the square button"); 243 | } 244 | if (Ps3.event.button_down.triangle) { 245 | moveClawServoDown = true; 246 | Serial.println("Started pressing the triangle button"); 247 | } 248 | if (Ps3.event.button_up.triangle) { 249 | moveClawServoDown = false; 250 | } 251 | 252 | if (Ps3.event.button_down.circle) { 253 | moveAuxServoDown = true; 254 | Serial.println("Started pressing the circle button"); 255 | } 256 | if (Ps3.event.button_up.circle) { 257 | moveAuxServoDown = false; 258 | Serial.println("Released the circle button"); 259 | } 260 | //--------------- Digital stick button events -------------- 261 | if (Ps3.event.button_down.l3) { 262 | if (!cabLightsOn) { 263 | digitalWrite(cabLights, HIGH); 264 | cabLightsOn = true; 265 | } else { 266 | digitalWrite(cabLights, LOW); 267 | cabLightsOn = false; 268 | } 269 | Serial.println("Started pressing the left stick button"); 270 | } 271 | if (Ps3.event.button_up.l3) 272 | Serial.println("Released the left stick button"); 273 | 274 | if (Ps3.event.button_down.r3) { 275 | if (!auxLightsOn) { 276 | digitalWrite(auxLights, HIGH); 277 | auxLightsOn = true; 278 | } else { 279 | digitalWrite(auxLights, LOW); 280 | auxLightsOn = false; 281 | } 282 | Serial.println("Started pressing the right stick button"); 283 | } 284 | if (Ps3.event.button_up.r3) { 285 | Serial.println("Released the right stick button"); 286 | } 287 | if (moveClawServoUp) { 288 | if (servoDelay == 3) { 289 | if (clawServoValue >= 10 && clawServoValue < 170) { 290 | clawServoValue = clawServoValue + 1; 291 | clawServo.write(clawServoValue); 292 | } 293 | servoDelay = 0; 294 | } 295 | servoDelay++; 296 | } 297 | if (moveClawServoDown) { 298 | if (servoDelay == 3) { 299 | if (clawServoValue <= 170 && clawServoValue > 10) { 300 | clawServoValue = clawServoValue - 1; 301 | clawServo.write(clawServoValue); 302 | } 303 | servoDelay = 0; 304 | } 305 | servoDelay++; 306 | } 307 | if (moveAuxServoUp) { 308 | if (servoDelay == 3) { 309 | if (auxServoValue >= 10 && auxServoValue < 170) { 310 | auxServoValue = auxServoValue + 1; 311 | auxServo.write(auxServoValue); 312 | } 313 | servoDelay = 0; 314 | } 315 | servoDelay++; 316 | } 317 | if (moveAuxServoDown) { 318 | if (servoDelay == 3) { 319 | if (auxServoValue <= 170 && auxServoValue > 10) { 320 | auxServoValue = auxServoValue - 1; 321 | auxServo.write(auxServoValue); 322 | } 323 | servoDelay = 0; 324 | } 325 | servoDelay++; 326 | } 327 | } 328 | 329 | void onConnect() { 330 | Serial.println("Connected."); 331 | } 332 | 333 | void setup() { 334 | 335 | Serial.begin(115200); 336 | 337 | mcp.begin_I2C(); 338 | // put your setup code here, to run once: 339 | 340 | 341 | for (int i = 0; i <= 15; i++) { 342 | mcp.pinMode(i, OUTPUT); 343 | } 344 | 345 | Ps3.attach(notify); 346 | Ps3.attachOnConnect(onConnect); 347 | Ps3.begin("a0:5a:5a:a0:0f:91"); 348 | 349 | Serial.println("Ready."); 350 | 351 | pinMode(clawServoPin, OUTPUT); 352 | pinMode(auxServoPin, OUTPUT); 353 | 354 | pinMode(cabLights, OUTPUT); 355 | pinMode(auxLights, OUTPUT); 356 | 357 | clawServo.attach(clawServoPin); 358 | auxServo.attach(auxServoPin); 359 | clawServo.write(clawServoValue); 360 | auxServo.write(auxServoValue); 361 | } 362 | 363 | 364 | 365 | void loop() { 366 | if (!Ps3.isConnected()) 367 | return; 368 | delay(500); 369 | } 370 | --------------------------------------------------------------------------------