While i use a joystick; going up and left will go forward sometimes and right other times while going right and down will go down sometimes and left other times
here is the code I'm using:
#include <Servo.h>
/*
White: Right X
Yellow: Right Y (A2)
Blue: Left Y (A1)
Green: Left X (A0)
*/
Servo esc_FL; // create servo object to control the ESC (Forward Left)
Servo esc_FR; // Forward Right
Servo esc_UL; // Up Left
Servo esc_UR; //Up Right
int PotVal_LX; //Left joystick values in the X direction
int PotVal_LY; //Left joystick values in the Y direction
int PotVal_RY; //Right joystick values in the Y direction
const int pingPin = 7; // Trigger Pin of Ultrasonic Sensor
const int echoPin = 8; // Echo Pin of Ultrasonic Sensor
int autoDist = 25; //Distance in cm that the ROV must be from the bottom of the pool for the autonomous task
int drive = 0; //variable that will either be 1 or 0, depending on whether driver has pushed the autonomous button
int buttonPin = 12; //Pin that is the input for the autonomous button signal
int PiLeft = 5;
int PiRight = 4;
int low = 65; //low end of pot
int high = 915; //high end of pot
int upperboundU = 460; //upperbound of pot for up motors (12V calb: 460)
int lowerboundU = 390; //lowerbound of pot for up motors (12V calb: 390)
int upperboundFX = 580; //upperbound of pot for fwd motors (12V calb: 550)
int lowerboundFX = 450; //lowerbound of pot for fwd motors (12V calb: 500)
int upperboundFY = 550; //upperbound of pot for fwd motors (12V calb: 550)
int lowerboundFY = 490; //lowerbound of pot for fwd motors (12V calb: 500)
int pwmV1; //just a var
int pwmV; //just a var
int pwmV2; //just a var
int pwmV3; //just a var
void setup() {
delay(1000);
// put your setup code here, to run once:
esc_FL.attach(6); //6 is the pin that the ESC is attached to
esc_FR.attach(9);
esc_UL.attach(10);
esc_UR.attach(11);
pinMode(buttonPin, INPUT);
pinMode(pingPin, OUTPUT);
pinMode(echoPin, INPUT);
Serial.begin(9600);
esc_FL.writeMicroseconds(1500);
esc_FR.writeMicroseconds(1500);
esc_UR.writeMicroseconds(1500);
esc_UL.writeMicroseconds(1500);
delay(7000); //Allow for ESC to calibrate
}
void loop() {
//while the LY potentiometer values are outside of the deadzone, run the forward motors accordingly
while(PotVal_LY < lowerboundFY or PotVal_LY < upperboundFY and drive == 0){
reading();
run_FY();
run_U();
//Serial.println("Driving Normal");
}
//while the LX potentiometer values are outside of the deadzone, run the forward motors accordingly
while(PotVal_LX < upperboundFX or PotVal_LX < lowerboundFX and drive == 0){
reading();
run_FX();
run_U();
//Serial.println("Turning");
}
/reason this isn't in a while loop is because if the LX and LY joysticks aren't in the other
while loops, than that means that they are both in the deadzones, and the forward motors shouldn't
be moving, so they are being told not to move/
reading();
esc_FL.writeMicroseconds(1500);
esc_FR.writeMicroseconds(1500);
run_U();
reading();
}
/*
The function takes in the joystick values in the x direction. Depending on how far the
joystick is moved, it will affect how fast the motors turn and in which direction.
*/
void run_FX(){
//if the joystick is in the deadzone, write to ESCs not to move
if (PotVal_LX > upperboundFX and PotVal_LX < lowerboundFX){
esc_FL.writeMicroseconds(1500);
esc_FR.writeMicroseconds(1500);
}
else{
if(PotVal_LX > 512){
pwmV = map(PotVal_LX, low, high, 1100, 1900); //take the potentiometer values that are between 65 and 915 and convert them to values between 1100 and 1900
pwmV1 = map(1023-PotVal_LX, low, high, 1100, 1900);
}
pwmV = map(1023-PotVal_LX, low, high, 1100, 1900); //take the potentiometer values that are between 65 and 915 and convert them to values between 1100 and 1900
pwmV1 = map(PotVal_LX, low, high, 1100, 1900);
esc_FL.writeMicroseconds(pwmV1); //write the mapped values to the ESCs
esc_FR.writeMicroseconds(pwmV);
}
}
void run_FY(){
//if the joystick is in the deadzone, write to ESCs not to move
if (PotVal_LY > upperboundFY and PotVal_LY < lowerboundFY){
esc_FL.writeMicroseconds(1500);
esc_FR.writeMicroseconds(1500);
}
else{
pwmV = map(PotVal_LY, 150, 770, 1100, 1900);
pwmV1 = map(PotVal_LY, 150, 770, 1100, 1900);
esc_FL.writeMicroseconds(pwmV1);
esc_FR.writeMicroseconds(pwmV);
}
}
void run_U(){
//if the joystick is in the deadzone, write to ESCs not to move
if (PotVal_RY > lowerboundU and PotVal_RY < upperboundU){
esc_UL.writeMicroseconds(1500);
esc_UR.writeMicroseconds(1500);
}
else{
pwmV2 = map(PotVal_RY, low, high, 1100, 1900);
pwmV3 = map(PotVal_RY, low, high, 1900, 1100);
esc_UL.writeMicroseconds(pwmV2);
esc_UR.writeMicroseconds(pwmV3);
}
}
void reading(){ //Function that is constantly updating sensor readings
PotVal_LX = analogRead(A0);
PotVal_RY = analogRead(A2);
PotVal_LY = analogRead(A1);
}