Notifications
Clear all

Show and Tell- A 4WD robot with nRF24L01 control  

Page 2 / 2

robotBuilder
(@robotbuilder)
Honorable Member
Joined: 2 years ago
Posts: 741
 

@rich1860

The pdf was sufficient to see the circuits.  I did also open the .doc on another computer that had msword.

 


ReplyQuote
Rich1860
(@rich1860)
Active Member
Joined: 3 months ago
Posts: 14
Topic starter  

 

Here is the code I used for my DIY Robot, transmitter. I call the transmitter the Drivers Controller,

This post was modified 2 weeks ago by Rich1860

ReplyQuote
Rich1860
(@rich1860)
Active Member
Joined: 3 months ago
Posts: 14
Topic starter  

Here is a copy of my code I used for my robot transmitter, or Driver Control. 

/*
DIY_Driver_Control_1.1

Version 1.1 adds the z axis.
VERSION 1.1.1, Changes CE and CSN from 8, 9 to 8, 10., Changed rate from
1 MBPS TO 2 MBPS

The is the Driver Control Program for the DIY Robot, and is the
radio transmitter for the DIY Robot Tele-op control.

The radio control program was developed from the program
"SaptarskiSider_Transmitter" found on YouTube. Set channel,
power level adjustment, were added and A0 and A1 were reversed
to make x and y axis correct, by R. Anderson, Team 4391,
Dundee ViBorgs, Mentor. The z axis capability was added.

CHANNEL SET AT 70. PLEASE REVISE THIS POST IF CHANNEL CHANGED.

Make sure the receiver is set to the same channel.

YOU HAVE TO INSTALL THE RF24 LIBRARY BEFORE UPLOADING THE CODE
https://github.com/tmrh20/RF24/

*/
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
RF24 radio(8,10); // CE, CSN, V 1.1.1 changed to 8, 10
const byte address[6] = "00001";
char xyData[32] = "";
String xAxis, yAxis, zAxis; // added zAxis
//String xAxis, yAxis
void setup() {
Serial.begin(9600);
radio.begin();
radio.setDataRate(RF24_2MBPS); //Increase data rate, choices are
//250 KBPS, 1MBPS, 2MBPS

radio.openWritingPipe(address);
radio.setChannel(70); //Set channel between 1-124
// Recommend 70-80 in USA
radio.setPALevel(RF24_PA_HIGH);

radio.stopListening();
}
void loop() {

xAxis = analogRead(A0); // Read Joysticks X-axis
yAxis = analogRead(A1); // Read Joysticks Y-axis

zAxis = analogRead(A2); // Read Z-axis
// X value
xAxis.toCharArray(xyData, 5); // Put the String (X Value) into a character array
radio.write(&xyData, sizeof(xyData)); // Send the array data (X value) to the other NRF24L01 modile
// Y value

yAxis.toCharArray(xyData, 5);
radio.write(&xyData, sizeof(xyData));
//delay(20);

zAxis.toCharArray(xyData, 5); //Add Z-axis
radio.write(&xyData, sizeof(xyData));
delay(20);

}


ReplyQuote
Rich1860
(@rich1860)
Active Member
Joined: 3 months ago
Posts: 14
Topic starter  

Here is the code for my robot, which includes the robot radio receiver.

 

/*
DIY Robot Program 2.1, THIS IS THE FINAL PROGRAM TO BE LOADED ONTO
THE DIY ROBOT, using the RF24 library for the radio control.

Version 2.1, added code for the ultrasonic distaqnce sensor HC-SR04.
Version 2.1.1, changed CE, CSN from 8, 9 to 8, 10, Changed rate from
1 MBPS TO 2 MBPS. Added the zAxis in the radio control for the pushbutton.
Added to the code for SwitchState4 to also make pin 23 High if zAxis
is greater than 200. 200 was chosen because zAxis is not quite zero
when the pushbutton is OFF, it runs around 23. If the pushbutton if ON,
the value of zAxis is 1023. Serial.println is added to read out the XAxis,
yAxis, and zAxis if troubleshooting of the radio is needed.

CHANNEL SET AT 70

This is intended to be the second program loaded into the Robot.
It will add Teleop capability or the ability to control the robot
from the drivers controller with radio control. The radio
transmission band is 2.5 GHz, it is low power so that it does
not require a license under the FCC (Federal Communications
Commission).

It is not intended to be loaded into the Driver Controller, which is
another program.

Version Notes:

Version 0, initial issue. This version adds the radio transmission
and receive capability to the robot, so that it can be driven
by a driver from a Driver Controller. This program is derived from
"ArduinoJOYSTICK CONTROLLED CAR (RECEIVER), adusted pin outs
for an Arduino Mega 2560, added channel selection (Set Channel),
reduced the value of enA and enB by .8 for slight speed reduction, and
added a z-axis for a third output pin (pin 11).

The program SaptarskiSikder has been tested and modifications made
by R. Anderson, Team 4395 Dundee ViBorgs, FIRST Robotics team,
Dundee Community Schools. An additional modification of this program
is to add capability for a 4 Wheel Drive (4WD) robot from a 2WD robot,
and to add a zAxis for a third control from the Driver Station.

DIY ROBOT PROGRAM 1

This program is written to test run the robot. It will only run the
robot in the autonomous mode, and will run the robot using interupts
on Mega pins 2 and 3. It will run all four wheels, forward, reverse,
right spin with rtight wheels forward and left wheels reverse,
and left turn with left wheels forward and right wheels off.
For Program 1, the test runs are disabled except for an initial
run forward. Other tests can be established if the comment
field around them is disabled. The programming for the switch control
of SW2, SW3, dand SW4 is also included for the initial testing.

THIS PROGRAM IS FOR CHECK OUT AND INITIAL OPERATION OF THE ROBOT
The Program No. 2 is intended to add the Ultrasonic control and
radio control Tele-Op mode of the robot

This program was copied from the RobotCarSpeedDemo from the
DroneBot Workshop described below, version 1.1.1.c.

These revisions were developed by R. C. Anderson,
for the Dundee ViBorgs robotic team.

DIY YOURSELF ROBOT PROGRAM 1 Original Version

RobotCarSpeedSensorDemo
Robot Car with Speed Sensor Demonstration
RobotCarSpeedSensorDemo.ino
Demonstrates use of Hardware Interrupts
to control motors on Robot Car

DroneBot Workshop 2017
https://dronebotworkshop.com

The HC-SR04 code:
HC-SR04 code V1.1
*
*Version 1.1, This code is modified to work on the Arduino Mega
* 2560, Trigger pin is 26, Echo pin is 28. Removed cm distance
* measurement.
*
* created by Rui Santos, https://randomnerdtutorials.com
*
* Complete Guide for Ultrasonic Sensor HC-SR04
*
Subsequent changes to this code, including adding the code for
robot control created by R. C. Anderson, Team 4395 Dundee ViBorgs.

VERSION 1.0-Initial version from DroneBot Workshop
Version 1.1, revised pin 5 to pin 11, so that PWM clock speed is
the same. Changed distances in test motor movement, # steps
to 100 from 50 to get one metre vs 1/2 metre, changed others to
get better test movement.
Version 1.1.1 Revised for the Arduino Mega pins. enA from 10
to 30, in1 from 9 to 32, in2from 8 to 34, enB from 11 to 40,
in3 from 7 to 36, in4 from 6 to 38.
Version 1.1.1c Add front motors, except for interrupt pins for
front motor. (Will use rear interrupt counter to determine distance
and speed. Spin Left was changed from running left wheels forward
and right wheels in reverse, to left wheels forward and right wheels off.
*/
//THE FOLLOWING CODE IS FOR THE HC-SR04

int trigPin = 26; // Trigger
int echoPin = 28; // Echo
long duration, inches;

/*
THE FOLLOWING ADDS RADIO CONTROL CAPABILITY
*/

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
RF24 radio(8, 10); // CE, CSN, changed to 8, 10 V 2.1.1
const byte address[6] = "00001";
char receivedData[32] = "";
int xAxis, yAxis, zAxis;//Add zAxis

//int xAxis, yAxis;
int motorSpeedA = 0;
int motorSpeedB = 0;
int motorSpeedC = 0;
int motorSpeedD = 0;
//int LED = 11; //Add pin 11 to LED
//This is for a future mod to add an LED light

/*
THE FOLLOWING WILL ADD SWITCH CONTROL TO THE PROGRAM

SWITCH 1 OR SW1 IS THE ON/OFF SWITCH FOR 6 VOLT POWER, THIS SWITCH
DOES NOT REQUIRE LOGIC CONTROL TO FUNCTION.

*/
// PIN 4 is associated with SW2, which is the autonamous control
// PIN 5 is associated with SW3, which is ON/OFF for the
//sonic collision avoidance control
// PIN 6 is associated with SW4, which controls a relay
//to turn on the whoop-whoop circuit

int switchState2 = 0; //For Switch 2 or SW2
int switchState3 = 0; // For SW3
int switchState4 = 0; // For SW4

// Constants for Interrupt Pins
// Change values if not using Arduino Uno

const byte MOTOR_A = 3; // Motor 2 Interrupt Pin- INT 1- Right Motor (RR)
const byte MOTOR_B = 2; // Motor 1 Interrupt Pin- INT 0- Left Motor (LR)

// Constant for steps in disk
const float stepcount = 20.00; // 20 Slots in disk, change if different

// Constant for wheel diameter
const float wheeldiameter = 66.10; // Wheel diameter = 66.1 mm = 2.5 in

// Integers for pulse counters
volatile int counter_A = 0;
volatile int counter_B = 0;

// Motor A (RR)

int enA = 30;
int in1 = 32;
int in2 = 34;

// Motor B (LR)

int enB = 40;
int in3 = 36;
int in4 = 38;

// Motor C (RF)

int enC = 31;
int in5 = 33;
int in6 = 35;

// Motor D (LF)

int enD = 41;
int in7 = 37;
int in8 = 39;

// Interrupt Service Routines

// Motor A pulse count ISR
void ISR_countA()
{
counter_A++; // increment Motor A counter value
}

// Motor B pulse count ISR
void ISR_countB()
{
counter_B++; // increment Motor B counter value
}

// Function to convert from centimeters to steps
int CMtoSteps(float cm) {

int result; // Final calculation result
float circumference = (wheeldiameter * 3.14) / 10; // Calculate wheel
//circumference in cm
float cm_step = circumference / stepcount; // CM per Step

float f_result = cm / cm_step; // Calculate result as a float
result = (int) f_result; // Convert to an integer
//(note this is NOT rounded)

return result; // End and return result

}

// Function to Move Forward
void MoveForward(int steps, int mspeed)
{
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

// Set Motor A(RR) forward
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);

// Set Motor B(LR) forward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);

// Set Motor C(RF) forward
digitalWrite(in5, HIGH);
digitalWrite(in6, LOW);

// Set Motor D (LF) forward
digitalWrite(in7, HIGH);
digitalWrite(in8, LOW);

// Go forward until step value is reached
while (steps > counter_A && steps > counter_B) {

if (steps > counter_A) {
analogWrite(enA, mspeed);
analogWrite(enC, mspeed);
} else {
analogWrite(enA, 0);
analogWrite(enC, 0);
}
if (steps > counter_B) {
analogWrite(enB, mspeed);
analogWrite(enD, mspeed);
} else {
analogWrite(enB, 0);
analogWrite(enD, 0);
}
}

// Stop when done
analogWrite(enA, 0);
analogWrite(enB, 0);
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero
analogWrite(enC, 0);
analogWrite(enD, 0);

}

 

// Function to Move in Reverse
void MoveReverse(int steps, int mspeed)
{
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

// Set Motor A (RR) reverse
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);

// Set Motor B (LR) reverse
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);

// Set Motor C (RF) reverse
digitalWrite(in5, LOW);
digitalWrite(in6, HIGH);

// Set Motor D (LF) reverse
digitalWrite(in7, LOW);
digitalWrite(in8, HIGH);

// Go in reverse until step value is reached
while (steps > counter_A && steps > counter_B) {

if (steps > counter_A) {
analogWrite(enA, mspeed);
analogWrite(enC, mspeed);
} else {
analogWrite(enA, 0);
analogWrite(enC, 0);
}
if (steps > counter_B) {
analogWrite(enB, mspeed);
analogWrite(enD, mspeed);
} else {
analogWrite(enB, 0);
analogWrite(enD, 0);
}
}

// Stop when done
analogWrite(enA, 0);
analogWrite(enB, 0);
analogWrite(enC, 0);
analogWrite(enD, 0);
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

}

// Function to Spin Right
void SpinRight(int steps, int mspeed)
{
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

// Set Motor A (RR) reverse
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);

// Set Motor C (RF) reverse
digitalWrite(in5, LOW);
digitalWrite(in6, HIGH);

// Set Motor B (LR) forward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);

// Set Motor D (LF) forward
digitalWrite(in7, HIGH);
digitalWrite(in8, LOW);

// Go until step value is reached
while (steps > counter_A && steps > counter_B) {

if (steps > counter_A) {
analogWrite(enA, mspeed);
analogWrite(enC, mspeed);
} else {
analogWrite(enA, 0);
analogWrite(enC, 0);
}
if (steps > counter_B) {
analogWrite(enB, mspeed);
analogWrite(enD, mspeed);
} else {
analogWrite(enB, 0);
analogWrite(enD, 0);
}
}

// Stop when done
analogWrite(enA, 0);
analogWrite(enB, 0);
analogWrite(enC, 0);
analogWrite(enD, 0);
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

}

// Function to Turn Left (spin left). This will run left
// wheels forward
// right wheels will not run
void SpinLeft(int steps, int mspeed)
{
counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

// Set Motor A (RR)forward
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);

// Set Motor C (RF)forward
digitalWrite(in5, HIGH);
digitalWrite(in6, LOW);

// Set Motor B (LR) off
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);

// Set Motor D (LR) reverse
digitalWrite(in7, LOW);
digitalWrite(in8, LOW);

// Go until step value is reached
while (steps > counter_A && steps > counter_B) {

if (steps > counter_A) {
analogWrite(enA, mspeed);
analogWrite(enC, mspeed);
} else {
analogWrite(enA, 0);
analogWrite(enC, 0);
}
if (steps > counter_B) {
analogWrite(enB, mspeed);
analogWrite(enD, mspeed);
} else {
analogWrite(enB, 0);
analogWrite(enD, 0);
}
}

// Stop when done
analogWrite(enA, 0);
analogWrite(enB, 0);
analogWrite(enC, 0);
analogWrite(enD, 0);

counter_A = 0; // reset counter A to zero
counter_B = 0; // reset counter B to zero

}

 

 

 

void setup()
{

//THE FOLLOWING IS THE SETUP FOR THE HC-SR04.

//Serial Port begin
Serial.begin (9600);
//Define inputs and outputs
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

//THE FOLLOWING IS FOR SWITCH 2,3, AND 4 CONTROL

pinMode (4, INPUT); //Sets PIN 4 as input, associated with SW2
pinMode (5, INPUT); //Sets PIN 5 as input, associated with SW3
pinMode (6, INPUT); //Sets PIN 6 as input, associated with SW4

// This code turns on the autonomous code
switchState2 = digitalRead (4); //Read the voltage value of PIN 4

if (switchState2 ==HIGH){ // Execute the autonomous mode
//within the if statement

//THE FOLLOWING IS FOR THE INTERRUPT AND MOTOR CONTROL

// Attach the Interrupts to their ISR's
attachInterrupt(digitalPinToInterrupt (MOTOR_A), ISR_countA, RISING);
// Increase counter A when speed sensor pin goes High

attachInterrupt(digitalPinToInterrupt (MOTOR_B), ISR_countB, RISING);
// Increase counter B when speed sensor pin goes High

//THE FOLLOWING IS TO TEST THE MOTOR MOVEMENT AND THE AUTONOMOUS MODE

//ESTABLISH THE AUTONOMOUS MODE RUN HERE

MoveForward(CMtoSteps(400), 150); // Forward 400 at 100 speed
delay(1000); // Wait one second

MoveReverse(300, 255); // Reverse 300 steps at 255 speed
delay(1000); // Wait one second
MoveForward(300, 150); // Forward 300 steps at 150 speed
delay(1000); // Wait one second
MoveReverse(CMtoSteps(254), 200); // Reverse 2.54 cm at 200 speed
delay(1000); // Wait one second

SpinRight(200, 255); // Spin right 50 steps at 255 speed
delay(1000); // Wait one second

SpinLeft(400, 175); // Spin left 60 steps at 175 speed
delay(1000); // Wait one second

MoveForward(10, 255); // Forward 10 step at 255 speed

}

/*
THE FOLLOWING IS FOR RADIO CONTROL OF THE ROBOT
*/

// pinMode (LED, OUTPUT); //Makes LED as an output PMW pin
pinMode(enA, OUTPUT); //Motor A- RR
pinMode(enB, OUTPUT); //Motor B - LR
pinMode(in1, OUTPUT); //Motor A- RR
pinMode(in2, OUTPUT); //Motor A- RR
pinMode(in3, OUTPUT); //Motor B- LR
pinMode(in4, OUTPUT); //Motor B- LR
pinMode(enC, OUTPUT); //Motor C- RF
pinMode(enD, OUTPUT); //Motor D- LF
pinMode(in5, OUTPUT); //Motor C- RF
pinMode(in6, OUTPUT); //Motor C- RF
pinMode(in7, OUTPUT); //Motor D- LF
pinMode(in8, OUTPUT); //Motor D- RF

Serial.begin(9600);
radio.begin();
radio.openReadingPipe(0, address);
radio.setChannel(70); //Set channel between 1-124
//Recommend 70-80 in USA
radio.setPALevel(RF24_PA_HIGH);
radio.setDataRate(RF24_2MBPS); //Set data rate to 1MBPS
radio.startListening();

}

void loop(){

//THE FOLLOWING IS FOR SWITCH CONTROL IN THE LOOP

switchState3 = digitalRead (5); // Read the voltage value of PIN 5
if (switchState3 == HIGH) {
// THIS CODE IS FOR THE HC-SR04 CODE

// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
digitalWrite(trigPin, LOW);
delayMicroseconds(5);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

// Read the signal from the sensor: a HIGH pulse whose
// duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);

// Convert the time into a distance

inches = (duration/2) * 0.0135; // Divide by 74 or multiply by 0.0135

Serial.print(inches);
Serial.print("in, ");

Serial.println();

if(inches >15) {

// Go Forward.
// Motor speeds are set to 150, max is 255.
// Set Motor A forward
digitalWrite(enA, LOW);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, 150);

// Set Motor B forward
digitalWrite(enB, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, 150);

// Set Motor C forward
digitalWrite(enC, LOW);
digitalWrite(in5, HIGH);
digitalWrite(in6, LOW);
analogWrite(enC, 150);

// Set Motor D forward
digitalWrite(enD, LOW);
digitalWrite(in7, HIGH);
digitalWrite(in8, LOW);
analogWrite(enD, 150);
}
else{
//brake the motor A
digitalWrite(enA, LOW);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(enA, LOW);
// Brake Motor B
digitalWrite(enB, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
digitalWrite(enB, LOW);

//Brake Motor C

digitalWrite(enC, LOW);
digitalWrite(in5, LOW);
digitalWrite(in6, LOW);
digitalWrite(enC, LOW);

//Brake Motor D
digitalWrite(enD, LOW);
digitalWrite(in7, HIGH);
digitalWrite(in8, LOW);
digitalWrite(enD, LOW);
delay(100);
}

if(inches <=15){

//Install interrupt reverse here

// Attach the Interrupts to their ISR's
attachInterrupt(digitalPinToInterrupt (MOTOR_A), ISR_countA, RISING);
// Increase counter A when speed sensor pin goes High

attachInterrupt(digitalPinToInterrupt (MOTOR_B), ISR_countB, RISING);
// Increase counter B when speed sensor pin goes High

MoveReverse(100, 255); // Reverse 100 steps at 255 speed
delay(500); // Wait one half second

SpinRight(100, 255); // Spin right 100 steps at 255 speed
delay(500); // Wait one half second

}

}

if(switchState3 == LOW) {
//Put the radio control loop software here
if (radio.available()) { // If the NRF240L01 module received data

Serial.println("radio received data");

radio.read(&receivedData, sizeof(receivedData)); // Read the data
//and put it into character array
xAxis = atoi(&receivedData[0]); // Convert the data from the character
//array (received X value) into integer
Serial.println("xAxis");
Serial.println(xAxis);

delay(10);
radio.read(&receivedData, sizeof(receivedData));
yAxis = atoi(&receivedData[0]);
Serial.println("yAxis");
Serial.println(yAxis);
delay(10);

//}
Serial.println("zAxis");
Serial.println(zAxis);
radio.read(&receivedData, sizeof(receivedData));
zAxis = atoi(&receivedData[0]);
delay(10);

}

if (!radio.available()) {
Serial.println("radio not receiving");
}

// X-axis used for forward and backward control
if (xAxis < 470) {
// Set Motor A backward
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// Set Motor B backward
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
// Set Motor C backward
digitalWrite(in5, LOW);
digitalWrite(in6, HIGH);
// Set Motor D backward
digitalWrite(in7, LOW);
digitalWrite(in8, HIGH);

// Convert the declining X-axis readings for going backward
//from 470 to 0 into 0 to 255 value for the PWM signal for
//increasing the motor speed
motorSpeedA = map(xAxis, 470, 0, 0, 255);

motorSpeedB = map(xAxis, 470, 0, 0, 255);

motorSpeedC = map(xAxis, 470, 0, 0, 255);

motorSpeedD = map(xAxis, 470, 0, 0, 255);

// Serial.println(motorSpeedA);
// Serial.println(motorSpeedB);
// Serial.println(motorSpeedC);
// Serial.println(motorSpeedD);

}
else if (xAxis > 550) {
// Set Motor A forward
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// Set Motor B forward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
// Set Motor C forward
digitalWrite(in5, HIGH);
digitalWrite(in6, LOW);
// Set Motor D forward
digitalWrite(in7, HIGH);
digitalWrite(in8, LOW);

// Convert the increasing X-axis readings for going forward
//from 550 to 1023 into 0 to 255 value for the PWM signal
//for increasing the motor speed
motorSpeedA = map(xAxis, 550, 1023, 0, 255);

//Serial.println(motorSpeedA);
motorSpeedB = map(xAxis, 550, 1023, 0, 255);

// Convert the increasing X-axis readings for going forward
//from 550 to 1023 into 0 to 255 value for the PWM signal
//for increasing the motor speed
motorSpeedC = map(xAxis, 550, 1023, 0, 255);

//Serial.println(motorSpeedA);
motorSpeedD = map(xAxis, 550, 1023, 0, 255);

}
// If joystick stays in middle the motors are not moving
else {
motorSpeedA = 0;
motorSpeedB = 0;
motorSpeedC = 0;
motorSpeedD = 0;
}
// Y-axis used for left and right control
if (yAxis < 470) {
// Convert the declining X-axis readings from
//470 to 0 into increasing 0 to 255 value
int yMapped = map(yAxis, 470, 0, 0, 255);
// Move to left - decrease left motor speed,
//increase right motor speed

motorSpeedA = motorSpeedA + yMapped;//A is right motor
motorSpeedB = motorSpeedB - yMapped;//B is left motor
motorSpeedC = motorSpeedC + yMapped;//C is right motor
motorSpeedD = motorSpeedD - yMapped;//D is left motor

// Confine the range from 0 to 255
if (motorSpeedA < 0) {
motorSpeedA = 0;
}
if (motorSpeedC < 0) {
motorSpeedC = 0;
}

if (motorSpeedB > 255) {
motorSpeedB = 255;
}

if (motorSpeedD > 255) {
motorSpeedD = 255;
}

}

if (yAxis > 550) {
// Convert the increasing Y-axis readings from
//550 to 1023 into 0 to 255 value
int yMapped = map(yAxis, 550, 1023, 0, 255);
// Move right - decrease right motor speed,
//increase left motor speed
motorSpeedA = motorSpeedA - yMapped;
motorSpeedB = motorSpeedB + yMapped;
motorSpeedC = motorSpeedC - yMapped;
motorSpeedD = motorSpeedD + yMapped;

// Confine the range from 0 to 255
if (motorSpeedA > 255) {
motorSpeedA = 255;
}
if (motorSpeedC > 255) {
motorSpeedC = 255;
}

if (motorSpeedB < 0) {
motorSpeedB = 0;
}

if (motorSpeedD < 0) {
motorSpeedD = 0;
}
}
// Prevent buzzing at low speeds (Adjust according to your
//motors. My motors couldn't start moving
//if PWM value was below value of 70)
if (motorSpeedA < 20) {
motorSpeedA = 0;
}
if (motorSpeedB < 20) {
motorSpeedB = 0;
}

// Prevent buzzing at low speeds (Adjust according to your
//motors. My motors couldn't start moving if PWM value
//was below value of 70)
if (motorSpeedC < 20) {
motorSpeedC = 0;
}
if (motorSpeedD < 20) {
motorSpeedD = 0;
}

analogWrite(enA, motorSpeedA*1.0); // Send PWM signal to motor A
analogWrite(enB, motorSpeedB*1.0 ); // Send PWM signal to motor B
analogWrite(enC, motorSpeedC*1.0); // Send PWM signal to motor C
analogWrite(enD, motorSpeedD*1.0 ); // Send PWM signal to motor D
/*THESE SERIAL PRINTS CAN BE ADDED BY REMOVING THE COMMENT.
Serial.println(enA);
Serial.println(motorSpeedA);

Serial.println(enB);
Serial.println(motorSpeedB);
Serial.println(enC);
Serial.println(motorSpeedC);
Serial.println(enD);
Serial.println(motorSpeedD);

*/
}

 

switchState4 = digitalRead (6); // Read the voltage value of PIN 6
if (switchState4 == HIGH) {
analogWrite(23, 255); //PIN 23 is the Whoop Whoop Relay,
// energize realy with 5.0 volts (255) to operate relay
}
else if (zAxis >200){
analogWrite(23, 255);
}

else{
analogWrite(23, 0); //De-energize Whoop Whoop Relay
}

}


ReplyQuote
robotBuilder
(@robotbuilder)
Honorable Member
Joined: 2 years ago
Posts: 741
 

@rich1860

If in your editor you can Select All and Copy as HTML when you paste to the forum the indentations and color text with be retained.  Such long code is almost unreadable otherwise.

 

 

 

This post was modified 2 weeks ago by robotBuilder

ReplyQuote
Page 2 / 2