I'd like to write code using a POT and a DC motor to control steering in a vehicle. When I command an integer steering angle between -30 degrees and 30 degrees in Python, I want the steering to adjust accordingly. I've written code that attempts to perform steering until reaching the desired POT sensor value, but it's not working well. Could you please help me with this?
The input from Python is as follows:
Steering angle, speed =
I used = to distinguish between multiple inputs overlapping each other.
For example, -30,255=-20,255=.
equalIdx and commaIdx are intended for that purpose.
I also added a condition to handle the case where communication with Python fails, causing Steer to become -1.
const int Handle1 = 4;
const int Handle2 = 5;
const int Left1 = 9;
const int Left2 = 8;
const int Right1 = 7;
const int Right2 = 6;
const int analogPin = A0;
const int left = 705; // -20
const int right = 488; // +20
const int middle = (left + right)/2;
const int tol = 5;
int vel = 0;
int Steer = 0;
int Steer_p = 0;
int Sensor = 596;
int Sensor_des = middle;
bool commsEnabled = true;
void setup() {
Serial.setTimeout(10);
pinMode(Handle1, OUTPUT);
pinMode(Handle2, OUTPUT);
pinMode(Left1, OUTPUT);
pinMode(Left2, OUTPUT);
pinMode(Right1, OUTPUT);
pinMode(Right2, OUTPUT);
Serial.begin(9600);
}
void loop() {
if (commsEnabled && Serial.available() > 0) {
Steering();
if (vel > 0){
Go();
}
else if (vel < 0){
Back();
}
else {
Stop();
}
delay(10);
Steer_p = Steer;
}
}
void Steering() {
Sensor = analogRead(analogPin);
delay(10);
if (Serial.available() > 0){
String input = Serial.readStringUntil('\n');
delay(10);
int commaIdx = input.indexOf(',');
int commaIdx2 = input.indexOf(',', commaIdx + 1);
int commaIdx3 = input.indexOf(',', commaIdx2 + 1);
int equalIdx = input.indexOf('=');
int equalIdx2 = input.indexOf('=', equalIdx + 1);
int equalIdx3 = input.indexOf('=', equalIdx + 1);
int length = input.length();
if (commaIdx3 > 0){
Steer = input.substring(equalIdx2 + 1, commaIdx3).toInt();
vel = input.substring(commaIdx3 + 1, length-1).toInt();
}
else if (commaIdx2 > 0){
Steer = input.substring(equalIdx + 1, commaIdx2).toInt();
vel = input.substring(commaIdx2 + 1, length-1).toInt();
}
else if (commaIdx > 0){
Steer = input.substring( 0, commaIdx).toInt();
vel = input.substring(commaIdx + 1, length-1 ).toInt();
}
if (Steer != -1){
Sensor_des = ((((left - right)/2) * (-Steer)) / 30) + middle;
}
else {
Sensor_des = ((((left - right)/2) * (-Steer_p)) / 30) + middle;
}
if (Steer == Steer_p){
return;
}
if (Sensor < Sensor_des){
while (Sensor < Sensor_des){
Left();
delay(10);
Sensor = analogRead(analogPin);
delay(10);
}
Forward();
}
else if (Sensor > Sensor_des){
while (Sensor > Sensor_des){
Right();
delay(10);
Sensor = analogRead(analogPin);
delay(10);
}
Forward();
}
else{
Forward();
Sensor = analogRead(analogPin);
}
delay(10);
return;
}
}
void Stop() {
digitalWrite(Left1, LOW);
digitalWrite(Left2, LOW);
digitalWrite(Right1, LOW);
digitalWrite(Right2, LOW);
}
void Go() {
int k = 5;
if (Steer < 0){
analogWrite(Left1, vel + (k*tan(Steer*PI/180)));
digitalWrite(Left2, LOW);
analogWrite(Right1, vel);
digitalWrite(Right2, LOW);
}
else{
analogWrite(Left1, vel);
digitalWrite(Left2, LOW);
analogWrite(Right1, vel - (k*tan(Steer*PI/180)));
digitalWrite(Right2, LOW);
}
}
void Back() {
int k = 5;
if (Steer < 0){
analogWrite(Left2, vel + (k*tan(Steer*PI/180)));
digitalWrite(Left1, LOW);
analogWrite(Right2, vel);
digitalWrite(Right1, LOW);
}
else{
analogWrite(Left2, vel);
digitalWrite(Left1, LOW);
analogWrite(Right2, vel - (k*tan(Steer*PI/180)));
digitalWrite(Right1, LOW);
}
}
void Left() {
int Steer_vel = 255 * abs(Steer_p - Steer) / 60;
if (Steer_vel > 255){
Steer_vel = 255;
}
if (Steer_vel < 150){
Steer_vel = 150;
}
analogWrite(Handle1,150);
digitalWrite(Handle2, LOW);
}
void Right() {
int Steer_vel = 255 * abs(Steer_p - Steer) / 60;
if (Steer_vel > 255){
Steer_vel = 255;
}
if (Steer_vel < 150){
Steer_vel = 150;
}
digitalWrite(Handle1, LOW);
analogWrite(Handle2,150);
}
void Forward() {
digitalWrite(Handle1, LOW);
digitalWrite(Handle2, LOW);
}
