Hardware components | ||||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
|
Another two-part project to add to my "What Do I Build Next?" series. The Osoyoo Mecanum platform is affordable now and can be coded using the Arduino and Raspberry Pi platforms.
I will start with the Arduino based platform, because that's where I learned and developed and spent the most time experimenting with code revisions to make my mecanum robot move as I expected it to.
This is link to Raspberry Pi based platform:
https://www.hackster.io/alien-energy-3/what-do-i-build-next-two-mecanum-bots-part-2-6fd7c6
I learned the hard way about software coding on both platforms, now you won't have to. I usually do not preface my projects with so much "historic info" but these two builds took over 6 months of my time to resolve all the coding issues that I discovered.
Hopefully, when you read and subsequently decide to possibly purchase either version of this platform, that the software/coding issues that I have noted and documented would have been resolved and incorporated by the Vendor.
I will in another Project listing continue with the Raspberry Pi 3B+ based platform, because that's where I used what I learned from this Arduino 2560 based mecanum bot and developed and spent almost three months experimenting with code revisions and troubleshooting wiring to make my mecanum robot move as I expected it to.
The Raspberry Pi based mecanum bot will require additional hardware not included in the Arduino based kit I first purchased in January of 2019, but hadn't built until earlier this year "2020".
Below are the webpage links that are required "as a guide, to successfully build, code, debug, and complete this functioning mecanum robot.
" https://osoyoo.com/2019/11/08/omni-direction-mecanum-wheel-robotic-kit-v1/ "
" https://osoyoo.com/2019/11/08/omni-direction-mecanum-wheel-robotic-kit-v1-lesson-2-tracking-line/ "
Before I explain and show the build, I want to list out lessons learned
1. Webpage links (NEXT, BACK, etc) may lead to dead pages or different versions of the product and text listing code hyperlink may be different than embedded hyperlink to same code ( I discovered different versions of code downloaded using the text weblink vs the wget... hyperlink) couldn't figure that one out at first but side by side comparison of downloaded code proved I wasn't insane.
2. Assembly descriptions, drawings, or illustrations may not be complete or applicable to your robot version/model
3. Downloaded code provided by Website owner may not work as expected or at all (move on to next part of build or use as a guide)
4. Rely on your experience to solve coding mistakes or anomalies; I found that these multi-lesson builds have code written by different "coders/individuals" and naming conventions for code snippets may change from lesson to lesson.
5. Make notes while troubleshooting code.
6. Pay attention to color of wires/jumpers and "pictured pin-out connections" versus the displayed table of interconnects.
7. If you plan on using the line following capability of this product, the webpage links are for five individual IR line sensors and the "wget" and "text" links are for five IR line sensors included in the OSOSYOO PiCAR V2.0 kit as well, BUT the Mecanum robot kit includes a three IR sensor module so you will need to revise that code.
8. Most importantly, I needed to reverse the motor power connections on my 2 pin motor connector on my rear two motors because all motors are wired the same polarity but on a 4 Wheeled robot the front motors are inversely mounted from the rear so the RED and BLACK wires need to be swapped. This is because the Osoyoo L298N Motor Pi module has K1 and K2 ports tied together, and K3 and K4 ports tied together. "WHY!!!!!" (I tried desperately to correct this with code but found that both motors would spin opposite of each other no matter what I tried). This wiring change is only required when using ONE L298N module. If using the two Motor-X modules then you can swap direction/rotation in code independently.
9. I had issues with the Osoyoo L298N Motor Pi module, it provides power to the Raspberry Pi at 5.1 volts and 2.5 amps via its battery pack of two 18650 3.7 volt batteries. The Motor-X L298N included with the Mecanum kit is designed for the Arduino so its output is not regulated down to Raspberry Pi specifications. I had one Motor Pi and Motor-X module that when powered ON would start spinning the left rear motor immediately while booting up. This initially was thought to be a wiring issue, a hung GPIO pin on my Raspberry Pi, or bad code, BUT the motor would function correctly when the code was running. It would start/stop/ and change direction of rotation as code requested but It was enabled HIGH when in the IDLE state. Since I had bought two Motor Pi modules I could swap it out and the issue went away. I am also running all four motors on the Model PI L298N module and not using the Model-X module at all.
Now for my Build Experience:
A. Research and Ordering
I had been monitoring Mecanum kits or wheels but were too costly. This kit's cost fluctuated +/- $20.00 USD. There are two similar kits offered by Osoyoo, one has plastic wheels and basic drive DC motors and this one with the Rubber wheels and geared motors with HALL sensors (but HALL sensors are unused). There is even a stripped down version without electronics. Pay attention to kit descriptions, features, and capabilities.
Plastic wheels
Arduino Rubber wheels
Basic platform
" https://www.amazon.com/OSOYOO-Robotic-Mecanum-Platform-Raspberry/dp/B07WZJYVB5/ref=pd_rhf_se_p_img_7?_encoding=UTF8&psc=1&refRID=RYPA66MSTEWV8RCDX6CS " for $79.99 USD.
B. Document Control- select documents for the Metal Chassis Mecanum Robot
After reviewing web links and documents, I had determined that I was initially following the wrong assembly and coding documents. I had thought my kit was the Mars Rover version that used the plastic wheels "Amazon's descriptions are very similiar", and I soon discovered that the Arduino Motor code examples were reversed from the Metal Mecanum version. After contacting Osoyoo's support through many emails, I determined the motor cables were wired differently thus code examples were incorrect. I also discovered that different Osoyoo's engineers had coded each development lesson, thus function naming schema were different and not expanded upon as the kit was built and features added.
C. Unpacking and Assembly
1. Gather hardware required for each assembly step- Robot Base with Motor Mounts
2. Attach Motor Mount to Robot Metal base plate
3. Now attach the remaining three motor mounts
4. Gather hardware for motors and wheels
5. Now attach other three motors
6. Gather hardware for wheel hub extension adapters
7. Now attach the other three wheel hubs
8. Now attach the wheels
9. Now lay out electronic module for standoff usage and installation
10. Now attach electronics and battery holder with plastic screws
It's now October 28, 2020; I've been busy with day-time paying job...but not to busy to keep thinking about this project's and the Raspberry Pi's version's write-up completion.
11. Installation of Ultrasonic sensor servo and top plate standoffs
12. Assembly of Ultrasonic Sensor and holder.
13. Wiring up motors and hardware to Interconnect Board
14. Motor control interconnect to UNO WIFI board
15. Voltage Meter and Battery interconnect
16. Ultrasonic Sensor and servo interconnect
17. Line Following sensor installation and interconnect
18. Bluetooth connection or WIFI use... you decide
19. Download Android App and let the fun begin
20. Software Testing Videos
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Lesson5 Wifi Control
* Tutorial URL http://osoyoo.com/?p=30022
* CopyRight www.osoyoo.com
*
*/
#define SPEED 85
#define TURN_SPEED 90
#define SHIFT_SPEED 130
#define TURN_TIME 500
#define MOVE_TIME 500
#define speedPinR 9 // Front Wheel PWM pin connect Right MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Right MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Right MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Front Left Motor direction pin 1 to Right MODEL-X IN3 (K3)
#define LeftMotorDirPin2 28 //Front Left Motor direction pin 2 to Right MODEL-X IN4 (K3)
#define speedPinL 10 // Front Wheel PWM pin connect Right MODEL-X ENB
#define speedPinRB 11 // Rear Wheel PWM pin connect Left MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Left MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Left MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear Left Motor direction pin 1 to Left MODEL-X IN3 (K3)
#define LeftMotorDirPin2B 8 //Rear Left Motor direction pin 2 to Left MODEL-X IN4 (K3)
#define speedPinLB 12 // Rear Wheel PWM pin connect Left MODEL-X ENB
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
FR_bck(speed_fr_bck);
RR_fwd(speed_rr_fwd);
;
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
FR_fwd(speed_fr_fwd);
RR_bck(speed_rr_bck);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_fwd(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_bck(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_fwd(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_bck(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_fwd(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_bck(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_fwd(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_bck(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
#include "WiFiEsp.h"
char ssid[] = "NETGEAROO"; // replace ****** with your network SSID (name)
char pass[] = "weijianweiye"; // replace ****** with your network password
int status = WL_IDLE_STATUS;
WiFiEspServer server(80);
// use a ring buffer to increase speed and reduce memory allocation
RingBuffer buf(8);
void setup()
{
init_GPIO();
Serial.begin(9600); // initialize serial for debugging
Serial1.begin(115200);
Serial1.write("AT+UART_DEF=9600,8,1,0,0\r\n");
delay(200);
Serial1.write("AT+RST\r\n");
delay(200);
Serial1.begin(9600); // initialize serial for ESP module
WiFi.init(&Serial1); // initialize ESP module
// check for the presence of the shield
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println("WiFi shield not present");
// don't continue
while (true);
}
// attempt to connect to WiFi network
while (status != WL_CONNECTED) {
Serial.print("Attempting to connect to WPA SSID: ");
Serial.println(ssid);
// Connect to WPA/WPA2 network
status = WiFi.begin(ssid, pass);
}
Serial.println("You're connected to the network");
printWifiStatus();
// start the web server on port 80
server.begin();
}
void loop()
{
WiFiEspClient client = server.available(); // listen for incoming clients
if (client) { // if you get a client,
Serial.println("New client"); // print a message out the serial port
buf.init(); // initialize the circular buffer
while (client.connected()) { // loop while the client's connected
if (client.available()) { // if there's bytes to read from the client,
char c = client.read(); // read a byte, then
buf.push(c); // push it to the ring buffer
// printing the stream to the serial monitor will slow down
// the receiving of data from the ESP filling the serial buffer
// Serial.write(c);
if (buf.endsWith("\r\n\r\n")) {
client.println("HTTP/1.1 200 OK");
client.println();
break;
}
// Check to see if the client request was "GET /H" or "GET /L":
if (buf.endsWith("?a=A")) {
//Serial.println("go Ahead");
go_advance(SPEED);
}
else if (buf.endsWith("?a=B")) {
go_back(SPEED);
}
else if (buf.endsWith("?a=L")) {
left_turn(TURN_SPEED);
}
else if (buf.endsWith("?a=R")) {
right_turn(TURN_SPEED);
}
else if (buf.endsWith("?a=E")) {
stop_Stop();
}
else if (buf.endsWith("?a=O")) {
left_shift(200,150,150,200);
} else if (buf.endsWith("?a=F")) {
right_shift(200,150,150,200);
} else if (buf.endsWith("?a=1")) {
left_shift(0,150,0,150); //left ahead
} else if (buf.endsWith("?a=4")) {
left_shift(150,0,150,0); //left back
} else if (buf.endsWith("?a=3")) {
right_shift(180,0,150,0); //right ahead
} else if (buf.endsWith("?a=6")) {
right_shift(0,130,0,130); //right back
}
}
}
// close the connection
client.stop();
Serial.println("Client disconnected");
}
}
void printWifiStatus()
{
// print the SSID of the network you're attached to
Serial.print("SSID: ");
Serial.println(WiFi.SSID());
// print your WiFi shield's IP address
IPAddress ip = WiFi.localIP();
Serial.print("IP Address: ");
Serial.println(ip);
// print where to go in the browser
Serial.println();
Serial.print("To see this page in action, open a browser to http://");
Serial.println(ip);
Serial.println();
}
/*
* How to test encoder with Arduino
* url: http://osoyoo.com/?p=30267
*/
#define outputA 6
#define outputB 7
int counter = 0;
int aState;
int aLastState;
void setup() {
pinMode (outputA,INPUT);
pinMode (outputB,INPUT);
Serial.begin (9600);
// Reads the initial state of the outputA
aLastState = digitalRead(outputA);
}
void loop() {
aState = digitalRead(outputA); // Reads the "current" state of the outputA
// If the previous and the current state of the outputA are different, that means a Pulse has occured
if (aState != aLastState){
// If the outputB state is different to the outputA state, that means the encoder is rotating clockwise
if (digitalRead(outputB) != aState) {
counter ++;
} else {
counter --;
}
Serial.print("Position: ");
Serial.println(counter);
}
aLastState = aState; // Updates the previous state of the outputA with the current state
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Bluetooth Testing Lesson
* Tutorial URL http://osoyoo.com/?p=30089
* CopyRight www.osoyoo.com
* After running the code, smart car will go forward 5 seconds, then go backward 5
* seconds, then left turn for 5 seconds then right turn for 5 seconds then stop.
*
*/
#define SPEED 140
#define TURN_SPEED 160
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
RR_fwd(speed_rr_fwd);
FR_bck(speed_fr_bck);
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
RR_bck(speed_rr_bck);
FR_fwd(speed_fr_fwd);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_fwd(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_bck(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_fwd(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_bck(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_fwd(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_bck(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_fwd(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_bck(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
void setup()
{
init_GPIO();
go_advance(SPEED);
delay(700);
stop_Stop();
delay(100);
go_back(SPEED);
delay(700);
stop_Stop();
delay(100);
left_turn(TURN_SPEED);
delay(700);
stop_Stop();
delay(100);
right_turn(TURN_SPEED);
delay(700);
stop_Stop();
delay(100);
right_shift(200,150,150,200); //right shift
delay(1000);
stop_Stop();
left_shift(200,150,150,200); //left shift
delay(1000);
stop_Stop();
left_shift(150,0,150,0); //left back
delay(1000);
stop_Stop();
right_shift(180,0,150,0); //right ahead
delay(1000);
stop_Stop();
left_shift(0,150,0,150); //left ahead
delay(1000);
stop_Stop();
right_shift(0,130,0,130); //right back
delay(1000);
stop_Stop();
}
void loop(){
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Omni direction Mecanum Wheel Arduino Smart Car Tutorial Lesson 2 Obstacle avoidance auto driving
* Tutorial URL http://osoyoo.com/?p=30219
* CopyRight www.osoyoo.com
* This project will show you how to make Osoyoo robot car in auto drive mode and avoid obstacles
*
*
*/
#include <Servo.h>
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
#define LPT 2 // scan loop coumter
#define SERVO_PIN 13 //servo connect to D5
#define Echo_PIN 31 // Ultrasonic Echo pin connect to A5
#define Trig_PIN 30 // Ultrasonic Trig pin connect to A4
#define FAST_SPEED 160 //both sides of the motor speed
#define SPEED 120 //both sides of the motor speed
#define TURN_SPEED 120 //both sides of the motor speed
#define BACK_SPEED1 160 //back speed
#define BACK_SPEED2 90 //back speed
int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval;
const int distancelimit = 30; //distance limit for obstacles in front
const int sidedistancelimit = 30; //minimum distance in cm to obstacles at both sides (the car will allow a shorter distance sideways)
int distance;
int numcycles = 0;
const int turntime = 250; //Time the robot spends turning (miliseconds)
const int backtime = 300; //Time the robot spends turning (miliseconds)
int thereis;
Servo head;
/*motor control*/
void go_Advance() //Forward
{
FR_fwd();
FL_fwd();
RR_fwd();
RL_fwd();
}
void go_Left() //Turn left
{
FR_fwd();
FL_bck();
RR_fwd();
RL_bck();
}
void go_Right() //Turn right
{
FR_bck();
FL_fwd();
RR_bck();
RL_fwd();
}
void go_Back() //Reverse
{
FR_bck();
FL_bck();
RR_bck();
RL_bck();
}
void stop_Stop() //Stop
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,LOW);
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,LOW);
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,LOW);
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,LOW);
set_Motorspeed(0,0,0,0);
}
/*set motor speed */
void set_Motorspeed(int leftFront,int rightFront,int leftBack,int rightBack)
{
analogWrite(speedPinL,leftFront);
analogWrite(speedPinR,rightFront);
analogWrite(speedPinLB,leftBack);
analogWrite(speedPinRB,rightBack);
}
void FR_fwd() //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
}
void FR_bck() // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
}
void FL_fwd() // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
}
void FL_bck() // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
}
void RR_fwd() //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
}
void RR_bck() //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
}
void RL_fwd() //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
}
void RL_bck() //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
}
/*detection of ultrasonic distance*/
int watch(){
long echo_distance;
digitalWrite(Trig_PIN,LOW);
delayMicroseconds(5);
digitalWrite(Trig_PIN,HIGH);
delayMicroseconds(15);
digitalWrite(Trig_PIN,LOW);
echo_distance=pulseIn(Echo_PIN,HIGH);
echo_distance=echo_distance*0.01657; //how far away is the object in cm
//Serial.println((int)echo_distance);
return round(echo_distance);
}
//Meassures distances to the right, left, front, left diagonal, right diagonal and asign them in cm to the variables rightscanval,
//leftscanval, centerscanval, ldiagonalscanval and rdiagonalscanval (there are 5 points for distance testing)
String watchsurrounding(){
/* obstacle_status is a binary integer, its last 5 digits stands for if there is any obstacles in 5 directions,
* for example B101000 last 5 digits is 01000, which stands for Left front has obstacle, B100111 means front, right front and right ha
*/
int obstacle_status =B100000;
centerscanval = watch();
if(centerscanval<distancelimit){
stop_Stop();
obstacle_status =obstacle_status | B100;
}
head.write(120);
delay(100);
ldiagonalscanval = watch();
if(ldiagonalscanval<distancelimit){
stop_Stop();
obstacle_status =obstacle_status | B1000;
}
head.write(170); //Didn't use 180 degrees because my servo is not able to take this angle
delay(300);
leftscanval = watch();
if(leftscanval<sidedistancelimit){
stop_Stop();
obstacle_status =obstacle_status | B10000;
}
head.write(90); //use 90 degrees if you are moving your servo through the whole 180 degrees
delay(100);
centerscanval = watch();
if(centerscanval<distancelimit){
stop_Stop();
obstacle_status =obstacle_status | B100;
}
head.write(40);
delay(100);
rdiagonalscanval = watch();
if(rdiagonalscanval<distancelimit){
stop_Stop();
obstacle_status =obstacle_status | B10;
}
head.write(0);
delay(100);
rightscanval = watch();
if(rightscanval<sidedistancelimit){
stop_Stop();
obstacle_status =obstacle_status | 1;
}
head.write(90); //Finish looking around (look forward again)
delay(300);
String obstacle_str= String(obstacle_status,BIN);
obstacle_str= obstacle_str.substring(1,6);
return obstacle_str; //return 5-character string standing for 5 direction obstacle status
}
void auto_avoidance(){
++numcycles;
if(numcycles>=LPT){ //Watch if something is around every LPT loops while moving forward
stop_Stop();
String obstacle_sign=watchsurrounding(); // 5 digits of obstacle_sign binary value means the 5 direction obstacle status
Serial.print("begin str=");
Serial.println(obstacle_sign);
if( obstacle_sign=="10000"){
Serial.println("SLIT right");
set_Motorspeed(FAST_SPEED,SPEED,FAST_SPEED,SPEED);
go_Advance();
delay(turntime);
stop_Stop();
}
else if( obstacle_sign=="00001" ){
Serial.println("SLIT LEFT");
set_Motorspeed(SPEED,FAST_SPEED,SPEED,FAST_SPEED);
go_Advance();
delay(turntime);
stop_Stop();
}
else if( obstacle_sign=="11100" || obstacle_sign=="01000" || obstacle_sign=="11000" || obstacle_sign=="10100" || obstacle_sign=="01100" ||obstacle_sign=="00100" ||obstacle_sign=="01000" ){
Serial.println("hand right");
go_Right();
set_Motorspeed(TURN_SPEED,TURN_SPEED,TURN_SPEED,TURN_SPEED);
delay(turntime);
stop_Stop();
}
else if( obstacle_sign=="00010" || obstacle_sign=="00111" || obstacle_sign=="00011" || obstacle_sign=="00101" || obstacle_sign=="00110" || obstacle_sign=="01010" ){
Serial.println("hand left");
go_Left();//Turn left
set_Motorspeed(TURN_SPEED,TURN_SPEED,TURN_SPEED,TURN_SPEED);
delay(turntime);
stop_Stop();
}
else if( obstacle_sign=="01111" || obstacle_sign=="10111" || obstacle_sign=="11111" ){
Serial.println("hand back left");
go_Back();
set_Motorspeed( BACK_SPEED1,BACK_SPEED2,BACK_SPEED1,BACK_SPEED2);
delay(backtime);
stop_Stop();
}
else if( obstacle_sign=="11011" || obstacle_sign=="11101" || obstacle_sign=="11110" || obstacle_sign=="01110" ){
Serial.println("hand back right");
go_Back();
set_Motorspeed(BACK_SPEED2,BACK_SPEED1,BACK_SPEED2,BACK_SPEED1);
delay(backtime);
stop_Stop();
}
else Serial.println("no handle");
numcycles=0; //Restart count of cycles
} else {
set_Motorspeed(SPEED,SPEED,SPEED,SPEED);
go_Advance(); // if nothing is wrong go forward using go() function above.
delay(backtime);
stop_Stop();
}
//else Serial.println(numcycles);
distance = watch(); // use the watch() function to see if anything is ahead (when the robot is just moving forward and not looking around it will test the distance in front)
if (distance<distancelimit){ // The robot will just stop if it is completely sure there's an obstacle ahead (must test 25 times) (needed to ignore ultrasonic sensor's false signals)
Serial.println("final go back");
go_Back();
set_Motorspeed(BACK_SPEED1,BACK_SPEED2,BACK_SPEED1,BACK_SPEED2);
delay(backtime);
++thereis;}
if (distance>distancelimit){
thereis=0;} //Count is restarted
if (thereis > 25){
Serial.println("final stop");
stop_Stop(); // Since something is ahead, stop moving.
thereis=0;
}
}
void setup() {
/*setup L298N pin mode*/
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();//stop move
/*init HC-SR04*/
pinMode(Trig_PIN, OUTPUT);
pinMode(Echo_PIN,INPUT);
/*init buzzer*/
digitalWrite(Trig_PIN,LOW);
/*init servo*/
head.attach(SERVO_PIN);
head.write(0);
delay(1000);
head.write(170);
delay(1000);
head.write(90);
delay(2000);
Serial.begin(9600);
stop_Stop();//Stop
}
void loop() {
auto_avoidance();
// Serial.println( watchsurrounding());
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car lesson 3: Line Tracking
* Tutorial URL http://osoyoo.com/2019/10/22/mecanum-omni-direction-wheel-robot-car-v2-0/
* CopyRight www.osoyoo.com
*
*/
#define Left_Pin 2
#define Center_Pin 3
#define Right_Pin 4
#define TURN_TIME 50
#define MOVE_TIME 50
#define SPEED 190
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
/*motor control*/
void right_shift(int speed) {
FL_fwd(speed);
RL_bck(speed);
RR_fwd(speed);
FR_bck(speed);
}
void left_shift(int speed){
FL_bck(speed);
RL_fwd(speed);
RR_bck(speed);
FR_fwd(speed);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void FR_fwd(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_bck(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_fwd(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_bck(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_fwd(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_bck(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_fwd(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_bck(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(Left_Pin, INPUT);
pinMode(Center_Pin, INPUT);
pinMode(Right_Pin, INPUT);
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
pinMode(Left_Pin, INPUT);
pinMode(Center_Pin, INPUT);
pinMode(Right_Pin, INPUT);
}
void setup()
{
init_GPIO();
Serial.begin(9600);
/*
go_advance(SPEED);
delay(1000);
go_back(SPEED);
delay(1000);
stop_Stop();
*/
}
void loop(){
auto_tracking();
}
String read_sensor_values()
{ int sensorvalue=8;
int sensor_left,sensor_center,sensor_right;
sensor_left= digitalRead(Left_Pin);
sensor_center=digitalRead(Center_Pin);
sensor_right=digitalRead(Right_Pin);
sensorvalue +=sensor_left*4+sensor_center*2+sensor_right;
String senstr= String(sensorvalue,BIN);
return senstr.substring(1,4);
}
void auto_tracking(){
String sensorval= read_sensor_values();
Serial.println(sensorval);
if (sensorval=="100" or sensorval=="110")
{
//The black line is in the left of the car, need left turn
Serial.println("TURN left");
left_turn(SPEED); //Turn right
delay(TURN_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="011" or sensorval=="001"){
//The black line is on the right of the car, need right turn
right_turn(SPEED); //Turn left
Serial.println("TURN right");
delay(TURN_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="010" or sensorval=="101"){
Serial.println("go ahead");
go_advance(SPEED);
delay(MOVE_TIME);
stop_Stop();
delay(50);
}
if (sensorval=="000" or sensorval=="111"){
Serial.println("back");
go_back(SPEED);
delay(MOVE_TIME/3*2);
stop_Stop();
delay(50);
}
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Bluetooth Testing Lesson
* Tutorial URL http://osoyoo.com/?p=30146
* CopyRight www.osoyoo.com
* After running the code, smart car will go forward 5 seconds, then go backward 5
* seconds, then left turn for 5 seconds then right turn for 5 seconds then stop.
*
*/
#include <SoftwareSerial.h>
#ifndef HAVE_HWSERIAL1
SoftwareSerial Serial1(4, 5); // if using UNO board, connect B_TX to D4,B_RX pin to D5, if use Mega2560, connect B_TX to RX1 D19,B_RX to TX1 D18,
#endif
#define MAX_PACKETSIZE 32 //Serial receive buffer
char buffUART[MAX_PACKETSIZE];
unsigned int buffUARTIndex = 0;
unsigned long preUARTTick = 0;
struct car_status{
int speed;
int angle;
int direct;
};
int move_speed=200 ;
#define SHIFT_SPEED 120
#define MIN_SPEED 100
#define MAX_SPEED 250
#define LEFT_SPEED 120
#define RIGHT_SPEED 120
int buttonState;
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
int car_direction = 1; // 1 means forward, 0 gear backward
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
RR_fwd(speed_rr_fwd);
FR_bck(speed_fr_bck);
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
RR_bck(speed_rr_bck);
FR_fwd(speed_fr_fwd);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_fwd(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_bck(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_fwd(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_bck(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_fwd(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_bck(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_fwd(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_bck(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
}
void setup()
{
init_GPIO();
Serial.begin(9600);//In order to fit the Bluetooth module's default baud rate, only 9600
Serial1.begin(9600);
stop_stop();
}
void loop(){
do_Uart_Tick();
}
void do_Uart_Tick()
{
char Uart_Date=0;
if(Serial1.available())
{
size_t len = Serial1.available();
uint8_t sbuf[len + 1];
sbuf[len] = 0x00;
Serial1.readBytes(sbuf, len);
//parseUartPackage((char*)sbuf);
memcpy(buffUART + buffUARTIndex, sbuf, len);//ensure that the serial port can read the entire frame of data
buffUARTIndex += len;
preUARTTick = millis();
if(buffUARTIndex >= MAX_PACKETSIZE - 1)
{
buffUARTIndex = MAX_PACKETSIZE - 2;
preUARTTick = preUARTTick - 200;
}
}
car_status cs;
if(buffUARTIndex > 0 && (millis() - preUARTTick >= 100))//APP send flag to modify the obstacle avoidance parameters
{ //data ready
buffUART[buffUARTIndex] = 0x00;
Uart_Date=buffUART[0];
cs=get_status(buffUART);
buffUARTIndex = 0;
}
move_speed=cs.speed+50;
if (cs.speed>MAX_SPEED) move_speed= MAX_SPEED;
switch (Uart_Date) //serial control instructions
{
case 'M':
car_direction=1;
go_advance(move_speed);
break;
case 'L':
car_direction=1;
left_turn(LEFT_SPEED);
break;
case 'R':
car_direction=1;
right_turn(RIGHT_SPEED);
break;
case 'B':
car_direction=0;
go_back(move_speed);
break;
case 'X':
car_direction=0;
left_back(move_speed);
break;
case 'Y':
car_direction=0;
right_back(move_speed);
break;
case 'F':
// left_shift(200,150,150,200); //left shift
left_shift(180,170,190,190) ;//left shift
delay(800);
break;
case 'J':
// right_shift(200,150,150,200); //right shift
right_shift(200,170,170,200); //right shift
delay(800);
break;
case 'G':
if (car_direction==1) left_shift(0,150,0,150); //left ahead
else left_shift(150,0,150,0); //left back
delay(800);
break;
case 'I':
if (car_direction==1) right_shift(180,0,150,0); //right ahead
else right_shift(0,130,0,130); //right back
delay(100);
break;
case 'E': stop_stop();
break;
default:break;
}
}
car_status get_status( char buffUART[])
{
car_status cstatus;
int index=2;
if (buffUART[index]=='-'){
cstatus.angle=-buffUART[index+1]+'0';
index=index+3;
} else {
cstatus.angle=buffUART[index]-'0';
index=index+2;
}
int currentvalue;
int spd=0;
while (buffUART[index]!=',')
{
currentvalue=buffUART[index]-'0';
spd=spd*10+currentvalue;
index++;
}
cstatus.speed=spd;
index++;
cstatus.direct=buffUART[index]-'0';
return cstatus;
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Lesson5 Wifi Control
* Tutorial URL http://osoyoo.com/?p=30176
* CopyRight www.osoyoo.com
*
*/
#define SPEED 85
#define TURN_SPEED 90
#define SHIFT_SPEED 130
#define TURN_TIME 500
#define MOVE_TIME 500
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
FR_bck(speed_fr_bck);
RR_fwd(speed_rr_fwd);
;
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
FR_fwd(speed_fr_fwd);
RR_bck(speed_rr_bck);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_bck(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_fwd(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_bck(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_fwd(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_bck(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_fwd(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_bck(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_fwd(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
#include "WiFiEsp.h"
#include "WiFiEspUDP.h"
char ssid[] = "montcruiser"; // replace ****** with your network SSID (name)
char pass[] = "AB19681216"; // replace ****** with your network password
int status = WL_IDLE_STATUS;
// use a ring buffer to increase speed and reduce memory allocation
char packetBuffer[5];
WiFiEspUDP Udp;
unsigned int localPort = 8888; // local port to listen on
void setup()
{
init_GPIO();
Serial.begin(9600); // initialize serial for debugging
Serial1.begin(115200);
Serial1.write("AT+UART_DEF=9600,8,1,0,0\r\n");
delay(200);
Serial1.write("AT+RST\r\n");
delay(200);
Serial1.begin(9600); // initialize serial for ESP module
WiFi.init(&Serial1); // initialize ESP module
// check for the presence of the shield
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println("WiFi shield not present");
// don't continue
while (true);
}
// attempt to connect to WiFi network
while (status != WL_CONNECTED) {
Serial.print("Attempting to connect to WPA SSID: ");
Serial.println(ssid);
// Connect to WPA/WPA2 network
status = WiFi.begin(ssid, pass);
}
Serial.println("You're connected to the network");
printWifiStatus();
Udp.begin(localPort);
Serial.print("Listening on port ");
Serial.println(localPort);
}
void loop()
{
int packetSize = Udp.parsePacket();
if (packetSize) { // if you get a client,
Serial.print("Received packet of size ");
Serial.println(packetSize);
int len = Udp.read(packetBuffer, 255);
if (len > 0) {
packetBuffer[len] = 0;
}
char c=packetBuffer[0];
switch (c) //serial control instructions
{
case 'A':go_advance(SPEED);;break;
case 'L':left_turn(TURN_SPEED);break;
case 'R':right_turn(TURN_SPEED);break;
case 'B':go_back(SPEED);break;
case 'E':stop_Stop();break;
case 'F':left_shift(0,150,0,150);break; //left ahead
case 'H':right_shift(180,0,150,0);break; //right ahead
case 'I':left_shift(150,0,150,0); break;//left back
case 'K':right_shift(0,130,0,130); break;//right back
case 'O':left_shift(200,150,150,200); break;//left shift
case 'T':right_shift(200,200,200,200); break;//left shift
default:break;
}
}
}
void printWifiStatus()
{
// print the SSID of the network you're attached to
Serial.print("SSID: ");
Serial.println(WiFi.SSID());
// print your WiFi shield's IP address
IPAddress ip = WiFi.localIP();
Serial.print("IP Address: ");
Serial.println(ip);
// print where to go in the browser
Serial.println();
Serial.print("To see this page in action, open a browser to http://");
Serial.println(ip);
Serial.println();
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Lesson5 Wifi Control
* Tutorial URL http://osoyoo.com/?p=30176
* CopyRight www.osoyoo.com
*
*/
#define SPEED 85
#define TURN_SPEED 90
#define SHIFT_SPEED 130
#define TURN_TIME 500
#define MOVE_TIME 500
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
FR_bck(speed_fr_bck);
RR_fwd(speed_rr_fwd);
;
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
FR_fwd(speed_fr_fwd);
RR_bck(speed_rr_bck);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_bck(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1, LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FR_fwd(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FL_bck(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void FL_fwd(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void RR_bck(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B, LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RR_fwd(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B, HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RL_bck(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void RL_fwd(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
#include "WiFiEsp.h"
#include "WiFiEspUDP.h"
char ssid[] = "osoyoo_robot";
int status = WL_IDLE_STATUS;
// use a ring buffer to increase speed and reduce memory allocation
char packetBuffer[5];
WiFiEspUDP Udp;
unsigned int localPort = 8888; // local port to listen on
void setup()
{
init_GPIO();
Serial.begin(9600); // initialize serial for debugging
Serial1.begin(115200);
Serial1.write("AT+UART_DEF=9600,8,1,0,0\r\n");
delay(200);
Serial1.write("AT+RST\r\n");
delay(200);
Serial1.begin(9600); // initialize serial for ESP module
WiFi.init(&Serial1); // initialize ESP module
// check for the presence of the shield
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println("WiFi shield not present");
// don't continue
while (true);
}
Serial.print("Attempting to start AP ");
Serial.println(ssid);
//AP mode
status = WiFi.beginAP(ssid, 10, "", 0);
Serial.println("You're connected to the network");
printWifiStatus();
Udp.begin(localPort);
Serial.print("Listening on port ");
Serial.println(localPort);
}
void loop()
{
int packetSize = Udp.parsePacket();
if (packetSize) { // if you get a client,
Serial.print("Received packet of size ");
Serial.println(packetSize);
int len = Udp.read(packetBuffer, 255);
if (len > 0) {
packetBuffer[len] = 0;
}
char c=packetBuffer[0];
switch (c) //serial control instructions
{
case 'A':go_advance(SPEED);;break;
case 'L':left_turn(TURN_SPEED);break;
case 'R':right_turn(TURN_SPEED);break;
case 'B':go_back(SPEED);break;
case 'E':stop_Stop();break;
case 'F':left_shift(0,150,0,150);break; //left ahead
case 'H':right_shift(180,0,150,0);break; //right ahead
case 'I':left_shift(150,0,150,0); break;//left back
case 'K':right_shift(0,130,0,130); break;//right back
case 'O':left_shift(200,150,150,200); break;//left shift
case 'T':right_shift(200,200,200,200); break;//left shift
default:break;
}
}
}
void printWifiStatus()
{
// print the SSID of the network you're attached to
Serial.print("SSID: ");
Serial.println(WiFi.SSID());
// print your WiFi shield's IP address
IPAddress ip = WiFi.localIP();
Serial.print("IP Address: ");
Serial.println(ip);
// print where to go in the browser
Serial.println();
Serial.print("To see this page in action, open a browser to http://");
Serial.println(ip);
Serial.println();
}
/* ___ ___ ___ _ _ ___ ___ ____ ___ ____
* / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \| \
*| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
* \___/(___/ \___/ \__ |\___/ \___(_)____)___/|_|_|_|
* (____/
* Arduino Mecanum Omni Direction Wheel Robot Car Lesson5 Wifi Control
* Tutorial URL http://osoyoo.com/?p=30022
* CopyRight www.osoyoo.com
*
*/
#define SPEED 85
#define TURN_SPEED 120
#define SHIFT_SPEED 130
#define TURN_TIME 500
#define MOVE_TIME 500
#define speedPinR 9 // Front Wheel PWM pin connect Right MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Right MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Right MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Front Left Motor direction pin 1 to Right MODEL-X IN3 (K3)
#define LeftMotorDirPin2 28 //Front Left Motor direction pin 2 to Right MODEL-X IN4 (K3)
#define speedPinL 10 // Front Wheel PWM pin connect Right MODEL-X ENB
#define speedPinRB 11 // Rear Wheel PWM pin connect Left MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Left MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Left MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear Left Motor direction pin 1 to Left MODEL-X IN3 (K3)
#define LeftMotorDirPin2B 8 //Rear Left Motor direction pin 2 to Left MODEL-X IN4 (K3)
#define speedPinLB 12 // Rear Wheel PWM pin connect Left MODEL-X ENB
/*motor control*/
void right_shift(int speed_fl_fwd,int speed_rl_bck ,int speed_rr_fwd,int speed_fr_bck) {
FL_fwd(speed_fl_fwd);
RL_bck(speed_rl_bck);
FR_bck(speed_fr_bck);
RR_fwd(speed_rr_fwd);
}
void left_shift(int speed_fl_bck,int speed_rl_fwd ,int speed_rr_bck,int speed_fr_fwd){
FL_bck(speed_fl_bck);
RL_fwd(speed_rl_fwd);
FR_fwd(speed_fr_fwd);
RR_bck(speed_rr_bck);
}
void go_advance(int speed){
RL_fwd(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_fwd(speed);
}
void go_back(int speed){
RL_bck(speed);
RR_bck(speed);
FR_bck(speed);
FL_bck(speed);
}
void left_turn(int speed){
RL_bck(0);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(0);
}
void right_turn(int speed){
RL_fwd(speed);
RR_bck(0);
FR_bck(0);
FL_fwd(speed);
}
void left_back(int speed){
RL_fwd(0);
RR_bck(speed);
FR_bck(speed);
FL_fwd(0);
}
void right_back(int speed){
RL_bck(speed);
RR_fwd(0);
FR_fwd(0);
FL_bck(speed);
}
void clockwise(int speed){
RL_fwd(speed);
RR_bck(speed);
FR_bck(speed);
FL_fwd(speed);
}
void countclockwise(int speed){
RL_bck(speed);
RR_fwd(speed);
FR_fwd(speed);
FL_bck(speed);
}
void FR_fwd(int speed) //front-right wheel forward turn
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
analogWrite(speedPinR,speed);
}
void FR_bck(int speed) // front-right wheel backward turn
{
digitalWrite(RightMotorDirPin1,LOW);
digitalWrite(RightMotorDirPin2,HIGH);
analogWrite(speedPinR,speed);
}
void FL_fwd(int speed) // front-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
analogWrite(speedPinL,speed);
}
void FL_bck(int speed) // front-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
analogWrite(speedPinL,speed);
}
void RR_fwd(int speed) //rear-right wheel forward turn
{
digitalWrite(RightMotorDirPin1B,HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
analogWrite(speedPinRB,speed);
}
void RR_bck(int speed) //rear-right wheel backward turn
{
digitalWrite(RightMotorDirPin1B,LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
analogWrite(speedPinRB,speed);
}
void RL_fwd(int speed) //rear-left wheel forward turn
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
analogWrite(speedPinLB,speed);
}
void RL_bck(int speed) //rear-left wheel backward turn
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
analogWrite(speedPinLB,speed);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
#include "WiFiEsp.h"
char ssid[] = "osoyoo_robot";
int status = WL_IDLE_STATUS;
WiFiEspServer server(80);
// use a ring buffer to increase speed and reduce memory allocation
RingBuffer buf(8);
void setup()
{
init_GPIO();
Serial.begin(9600); // initialize serial for debugging
Serial1.begin(115200);
Serial1.write("AT+UART_DEF=9600,8,1,0,0\r\n");
delay(200);
Serial1.write("AT+RST\r\n");
delay(200);
Serial1.begin(9600); // initialize serial for ESP module
WiFi.init(&Serial1); // initialize ESP module
// check for the presence of the shield
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println("WiFi shield not present");
// don't continue
while (true);
}
Serial.print("Attempting to start AP ");
Serial.println(ssid);
status = WiFi.beginAP(ssid, 10, "", 0);
Serial.println("You're connected to the network");
printWifiStatus();
// start the web server on port 80
server.begin();
}
void loop()
{
WiFiEspClient client = server.available(); // listen for incoming clients
if (client) { // if you get a client,
Serial.println("New client"); // print a message out the serial port
buf.init(); // initialize the circular buffer
while (client.connected()) { // loop while the client's connected
if (client.available()) { // if there's bytes to read from the client,
char c = client.read(); // read a byte, then
buf.push(c); // push it to the ring buffer
// printing the stream to the serial monitor will slow down
// the receiving of data from the ESP filling the serial buffer
// Serial.write(c);
if (buf.endsWith("\r\n\r\n")) {
client.println("HTTP/1.1 200 OK");
client.println();
break;
}
// Check to see if the client request was "GET /H" or "GET /L":
if (buf.endsWith("?a=A")) {
//Serial.println("go Ahead");
go_advance(SPEED);
}
else if (buf.endsWith("?a=B")) {
go_back(SPEED);
}
else if (buf.endsWith("?a=L")) {
left_turn(TURN_SPEED);
}
else if (buf.endsWith("?a=R")) {
right_turn(TURN_SPEED);
}
else if (buf.endsWith("?a=E")) {
stop_Stop();
}
else if (buf.endsWith("?a=O")) {
left_shift(180,170,170,180);
} else if (buf.endsWith("?a=F")) {
right_shift(180,170,170,180);
} else if (buf.endsWith("?a=1")) {
left_shift(0,170,0,170); //left ahead
} else if (buf.endsWith("?a=4")) {
left_shift(170,0,170,0); //left back
} else if (buf.endsWith("?a=3")) {
right_shift(170,0,170,0); //right ahead
} else if (buf.endsWith("?a=6")) {
right_shift(0,170,0,170); //right back
}
}
}
// close the connection
client.stop();
Serial.println("Client disconnected");
}
}
void printWifiStatus()
{
// print the SSID of the network you're attached to
Serial.print("SSID: ");
Serial.println(WiFi.SSID());
// print your WiFi shield's IP address
IPAddress ip = WiFi.localIP();
Serial.print("IP Address: ");
Serial.println(ip);
// print where to go in the browser
Serial.println();
Serial.print("To see this page in action, open a browser to http://");
Serial.println(ip);
Serial.println();
}
Comments