Code for 3 Omni Wheels Robot Car

Hello everyone

I have written a code for a 2 omni wheels robot car last week and now i want to change and rewrite the code for a 3 omni wheel robot car. I was wondering if it is legal here in this froum to ask for help of anyone to do this for me.

The code for my 2 omni wheel robot car is :

#include <Servo.h>
 
// Pins
#define TRIG_PIN A0
#define ECHO_PIN A1
 
//Define all the connections maps to the L298N
#define enA 13
#define in1 12
#define in2 11
#define in3 7
#define in4 6
#define enB 5
#define servoPin 2
 
class Motor{
 
int enablePin;
int directionPin1;
int directionPin2;
 
public:
 
//Method to define the motor pins
Motor(int ENPin,int dPin1,int dPin2){
enablePin = ENPin;
directionPin1 = dPin1;
directionPin2 = dPin2;
};
 
//Method to drive the motor 0~255 driving forward. -1~-255 driving backward
void Drive(int speed){
if(speed>=0){
digitalWrite(directionPin1, LOW);
digitalWrite(directionPin2, HIGH);
}
else{
digitalWrite(directionPin1, HIGH);
digitalWrite(directionPin2, LOW);
speed = - speed;
}
analogWrite(enablePin, speed);
}
};
 
Motor leftMotor = Motor(enA, in1, in2);
Motor rightMotor = Motor(enB, in3, in4);
Servo myservo; // create servo object to control a servo
 
void motorInitiation(){
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
// Set initial direction and speed
digitalWrite(enA, LOW);
digitalWrite(enB, LOW);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
 
//Variables--------------------------------------------------------------------------
// Anything over 400 cm (23200 us pulse) is "out of range"
const unsigned int MAX_DIST = 23200;
bool ObsticalAhead = false;
int servoPos = 90;
 
enum Directions { Forward, TurnLeft, TurnRight, TurnAround,Brake};
 
Directions nextStep = Forward;
 
unsigned long t1;
unsigned long t2;
unsigned long pulse_width;
float cm;
float inches;
 
//SETUP--------------------------------------------------------------------------
void setup() {
 
// The Trigger pin will tell the sensor to range find
pinMode(TRIG_PIN, OUTPUT);
digitalWrite(TRIG_PIN, LOW);
 
// We'll use the serial monitor to view the sensor output
Serial.begin(9600);
myservo.attach(servoPin);
motorInitiation();
Directions nextStep = Forward;
}
 
void loop() {
 
checkDistance();
checkDirection();
drive();
}
 
void checkDistance(){
 
// Hold the trigger pin high for at least 10 us
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
 
// Wait for pulse on echo pin
while ( digitalRead(ECHO_PIN) == 0 );
 
// Measure how long the echo pin was held high (pulse width)
// Note: the micros() counter will overflow after ~70 min
t1 = micros();
while ( digitalRead(ECHO_PIN) == 1);
t2 = micros();
pulse_width = t2 - t1;
 
// Calculate distance in centimeters and inches. The constants
// are found in the datasheet, and calculated from the assumed speed
//of sound in air at sea level (~340 m/s).
cm = pulse_width / 58.0;
inches = pulse_width / 148.0;
 
// Print out results
if ( pulse_width > MAX_DIST ) {
//Serial.println("Out of range");
} else {
//Serial.print(cm);
//Serial.print(" cm \t");
//Serial.print(inches);
//Serial.println(" in");
}
 
// Wait at least 60ms before next measurement
delay(60);
 
if(cm<= 20){
ObsticalAhead = true;
Serial.println("Problem Ahead");
 
}
else{ ObsticalAhead = false;}
 
}
void checkDirection(){
Serial.println("checking direction");
if(ObsticalAhead ==true){
nextStep = Brake;
drive();
myservo.write(180); // tell servo to go to position in variable 'pos'
delay(400); // waits 15ms for the servo to reach the position
checkDistance();
if(ObsticalAhead ==false){//if left side is open
nextStep = TurnLeft;
Serial.println("Next step is TurnLeft");
myservo.write(90);//reset servo position
delay(400);
}
else{// left is blocked, now need to look at right
myservo.write(0); // tell servo to go to position in variable 'pos'
delay(800); // waits 15ms for the servo to reach the position
checkDistance();
if(ObsticalAhead ==false){//if right side is open
nextStep = TurnRight;
Serial.println("Next step is TurnRight");
myservo.write(90);//reset servo position
delay(400);
}
else{//right is blocked as well, need to turn around
nextStep = TurnAround;
myservo.write(90);//reset servo position
delay(300);
Serial.println("Next step is TurnAround");
}
}
 
}
else{nextStep = Forward;}//No obstical ahead
}
 
void drive(){
switch (nextStep){
case Forward:
leftMotor.Drive(255);
rightMotor.Drive(255);
Serial.println("Forward");
 
break;
case TurnLeft:
leftMotor.Drive(-255);
rightMotor.Drive(255);
Serial.println(" TurnLeft");
delay(400);
 
break;
case TurnRight:
leftMotor.Drive(255);
rightMotor.Drive(-255);
Serial.println(" TurnRight");
delay(400);
break;
case TurnAround:
leftMotor.Drive(255);
rightMotor.Drive(-255);
Serial.println(" TurnAround");
delay(600);
break;
 
case Brake:
leftMotor.Drive(0);
rightMotor.Drive(0);
Serial.println(" stopped");
}
 
}

In the Gigs and Collaboration section, it is. There will be more interest if you offer to pay.

Do not start another thread, there, though. Click the Report to moderator link, and ask that this one be moved.

What pins are you going to use for the third motor driver?

Thread moved as requested. Thank you all!

Did u find the code to move the robot ? I need help

SamSam1919:
Did u find the code to move the robot ? I need help

post your code, using code tags, tell us what doesn't work, and think of using the right sub-forum, not here, unless you want to pay someone