Embedded Files
Problem:
Wild fires spread fast and cause a danger for the environment, humans and animals. That is why our bot was designed to put out forest fires and safly retrieve wildlife into safty.
Final Disign Sketch
Flowchart
Code:
#include <Servo.h> // Import Servo library
/* -------------------- Drive -------------------- */
const int kLeftWheel_Backwards = 3; // Left wheel backward pin
const int kLeftWheel_Forwards = 5; // Left wheel forward pin
const int kRightWheel_Forwards = 6; // Right wheel forward pin
const int kRightWheel_Backwards = 11; // Right wheel backward pin
const float leftWheelSpeed = 255; // Left wheel max speed
const float rightWheelSpeed = 255; // Right wheel max speed
const float deadZone = 0.15; // Joystick deadzone
bool speedPressed = false; // Speed button pressed
int currentSpeed = 0; // Current speed
float speeds[3] = {1, 0.7, 0.4}; // Speed settings
/* -------------------- Drive -------------------- */
/* -------------------- Arm -------------------- */
const int servo1Pin = 2; // Servo 1 pin
const int servo2Pin = 4; // Servo 2 pin
const float armSpeed = 2; // Arm speed
Servo servo1; // Servo 1 object
Servo servo2; // Servo 2 object
float angle = 0; // Horizontal angle
/* -------------------- Arm -------------------- */
/* -------------------- Controller -------------------- */
// extraButtons:
// btnLeftShoulder = 0x01
// btnRightShoulder = 0x02
// btnMidLeft = 0x04
// btnMidRight = 0x08
// btnRightJoy = 0x10
// btnLeftJoy = 0x20
// faceButtons:
// btnLeftUp = 0x01
// btnLeftRight = 0x02
// btnLeftDown = 0x04
// btnLeftLeft = 0x08
// btnRightUp = 0x10
// btnRightRight = 0x20
// btnRightDown = 0x40
// btnRightLeft = 0x80
uint8_t buffer[7] = {}; // Controller buffer
int numBytes = 0; // Number of read bytes
uint8_t extraButtons = 0; // Extra button data
uint8_t faceButtons = 0; // Face button data
uint8_t joystick[4] = {127, 127, 127, 127}; // Joystick data
void updateController() { // Get an update from the controller
if (numBytes != 7) { // If not 7 bytes read
while (Serial.available() > 0) { // While data is available
uint8_t byte = Serial.read(); // Read data
if (numBytes == 0 && byte != 0xFF) { // If empty message
continue; // Continue
} else if (numBytes < 7) { // If not 7 bytes read
buffer[numBytes++] = byte; // Save the byte
} else { // Else
memset(buffer, 0, sizeof(buffer)); // Reset buffer
numBytes = 0; // Reset num bytes read
}
}
delay(8); // Wait 8 ms
} else { // Else
extraButtons = buffer[1]; // Set extra button data to buffer[1]
faceButtons = buffer[2]; // Set face button data to buffer[2]
for (int i = 0; i < 4; i++) { // For i in 0-3
joystick[i] = buffer[i + 3]; // Set joystick data at i to buffer[i + 3]
}
memset(buffer, 0, sizeof(buffer)); // Reset buffer
numBytes = 0; // Reset num bytes read
}
}
/* -------------------- Controller -------------------- */
void setLeftMotor(int speed) { // Set left motor speed
int s = constrain(speed, -255, 255); // Constrain speed
if (s > 0) { // If speed positive
analogWrite(kLeftWheel_Forwards, s); // Set left wheel forward
analogWrite(kLeftWheel_Backwards, 0); // Dont set left wheel backward
} else if (s < 0) { // If speed negative
analogWrite(kLeftWheel_Forwards, 0); // Dont set left wheel forward
analogWrite(kLeftWheel_Backwards, -s); // Set left wheel backward
} else { // Else
analogWrite(kLeftWheel_Forwards, 0); // Dont set left wheel forward
analogWrite(kLeftWheel_Backwards, 0); // Dont set left wheel backward
}
}
void setRightMotor(int speed) { // Set right wheel speed
int s = constrain(speed, -255, 255); // Constrain speed
if (s > 0) { // If speed positive
analogWrite(kRightWheel_Forwards, s); // Set right wheel forward
analogWrite(kRightWheel_Backwards, 0); // Dont set right wheel backward
} else if (s < 0) { // If speed negative
analogWrite(kRightWheel_Forwards, 0); // Dont set right wheel forward
analogWrite(kRightWheel_Backwards, -s); // Dont set right wheel backward
} else { // Else
analogWrite(kRightWheel_Forwards, 0); // Dont set right wheel forward
analogWrite(kRightWheel_Backwards, 0); // Dont set right wheel backward
}
}
float normalizeJoystick(uint8_t stick) { // Normalize joystick input
double normalized = float(stick) / 128 - 1; // Normalize input
if (abs(normalized) < deadZone) { normalized = 0; } // Set to 0 if in deadzone
return normalized; // Return normalized
}
void setup() { // Setup, runs once
pinMode(kRightWheel_Backwards, OUTPUT); // Set right wheel backward pin
pinMode(kRightWheel_Forwards, OUTPUT); // Set right wheel forward pin
pinMode(kLeftWheel_Forwards, OUTPUT); // Set left wheel forward pin
pinMode(kLeftWheel_Backwards, OUTPUT); // Set left wheel backward pin
servo1.attach(servo1Pin); // Set servo 1 pin
servo2.attach(servo2Pin); // Set servo 2 pin
servo1.write(0); // Reset servo 1
servo2.write(0); // Reset servo 2
Serial.begin(9600); // Start serial
}
void loop() { // Loop, runs repeatedly
updateController(); // Update controller
bool rightDown = extraButtons & 0x20; // Is right button down
if (rightDown && !speedPressed) { // If right button down and not pressed
speedPressed = true; // Button pressed
currentSpeed = (currentSpeed + 1) % 3; // Switch speed
} else if (!rightDown) { // If right button not down
speedPressed = false; // Button not pressed
}
float driveNormX = normalizeJoystick(joystick[2]);
float driveNormY = normalizeJoystick(joystick[3]);
float len = sqrt(sq(driveNormX) + sq(driveNormY));
if (len > 1) {
driveNormX /= len;
driveNormY /= len;
}
setLeftMotor(leftWheelSpeed * (driveNormY + driveNormX) * speeds[currentSpeed]);
setRightMotor(rightWheelSpeed * (driveNormY - driveNormX) * speeds[currentSpeed]);
float clawNormY = normalizeJoystick(joystick[1]);
angle -= armSpeed * clawNormY * speeds[currentSpeed];
angle = constrain(angle, 90, 180);
servo1.write(angle); // Set servo 1
servo2.write(angle); // Set servo 2
}
Opportunities To Improve:
Our bot perfomed just as we had hoped but the bucket could have benefited from more degrees of freedom. Our final disign only had one servo for manipulation because when we explored other disigns we found having longer arms and more servos caused unriliability moving. We ended up simplifying our disign due to the deadline but if we had more time I would try and find a way to impliment our original disign and make it work as intended.
My Contribution
I did the CAD for our original disign and modified it into the bracket that we used to hold our servo. I also help complete the Engineering drawings and disigns.
Page updated
Google Sites
Report abuse