Hi,
I am currently working on a radio controlled robot tracked chassis with a robotic arm. I have 3d printed a controller for the robotic arm rather than just using a 'bank' of variable resistors.
I am really pleased with how everything is working, the code is stable and the arm works really nicely. That is until I add some code to control the jaws on the arm.
The jaws open and close with push button switches, one side connected to ground thru 10k resistors. On the transmit ( crawler base) side I have debugging information which is telling me that everything is working as it should.
However, on the receive (crawler) side there is no received data.
The data doesn't get received when I add the code for the bush button switches.
Code for transmitter side:
/*
* Sketch for :
* V7 Adding jaws open and close
* V6 Is not of any use
* V5A Cleaning up radio tx rx
* V5 Tweaking steering
* V4 Test using joystick for rotate CCW and CW, no buttons
* V3 Including 3 pos switch for video switching
* Sending steering joystick fwd/rev data
* Push buttons to rotate crawler on its axis
* Sending arm controller data including push button jaws
*/
//#include <SPI.h> //
#include <nRF24L01.h> //
#include <RF24.h> // Including radio Libraries
#include <Wire.h> //I2C serial comms
RF24 radio(9, 10); // CE, CSN
const byte address[6] = "00001"; //set array which contains the addrress
int Speeds[12]; // Array for sending data
int LTSpeed;
int RTSpeed;
int LeftX;
int RightX;
int JoystickX; // Define variables
int PotVal = 0;
int Speed;
int GearPin = 2; // Define digital pin 2
int FwdRev = 0;
int RotCW = 0;
int RotCCW = 0;
int SwitchPos1 = 5 ; //3 pos switch for camera switching
int SwitchPos2 = 6;
int PosPWM = 0;
int JawCloseButton = 7;
int JawOpenButton = 8;
int JawOpen; // specify variables
int JawClose;
int JawPos= 45;
int controlBase;
int controlShoulder;
int controlElbow; // Integers for reading pot values
int controlWrist;
int controlRotate;
//int controlJaws;
int mtrDegreeBase;
int mtrDegreeShoulder;
int mtrDegreeElbow;
int mtrDegreeWrist; //integers for how many degrees to move servos
int mtrDegreeRotate;
//int mtrDegreeJaws;
void setup() {
Serial.begin(115200);
pinMode(GearPin, INPUT);
pinMode( SwitchPos1, INPUT);
pinMode( SwitchPos2, INPUT);
pinMode(JawCloseButton, INPUT);
pinMode(JawOpenButton, INPUT);
radio.begin();
radio.openWritingPipe(address); // Define reading and writing addresses
radio.setPALevel(RF24_PA_LOW);
radio.setDataRate(RF24_250KBPS);
radio.setChannel(125);
radio.stopListening();
}
void loop() {
JoySend();
ArmSend();
CamSwitch();
if ((digitalRead(JawCloseButton) ==HIGH)||(digitalRead(JawOpenButton) ==HIGH))
{
JawSend(); }
radio.write(&Speeds, sizeof(Speeds)); //Send the array
}
void JoySend(){
PotVal = analogRead(A1); // Read the pot value which is the speed
Speed = map(PotVal, 0, 1023, 0, 255); //Map potvalue to 255
JoystickX = analogRead(A0); // Read the Left Right movement of joystick
if (JoystickX <=498) {
LeftX = map(JoystickX, 498, 2, 0, Speed); //Clever bit here, map to Speed value
RightX = 0;
}
if (JoystickX >= 498) {
RightX = map(JoystickX, 498, 1022, 0, Speed);
LeftX = 0;
}
LTSpeed = Speed - 1.1*RightX; //making LTSpeed go slightly negative for ccw
RTSpeed = Speed - 1.1*LeftX; // making RTSpeed go slightly negative for ccw
if ( LTSpeed<0){
RotCCW = HIGH;
}
else {
RotCCW = LOW;
}
if (RTSpeed<0) {
RotCW = HIGH ;
}
else {
RotCW = LOW;
}
FwdRev = digitalRead(GearPin);
Speeds[0] = LTSpeed; //Write the LTSpeed into the Speeds array slot 1
Speeds[1] = RTSpeed; //Write the RTSpeed into the Speeds array slot 2
Speeds[2] = FwdRev;
Speeds[3] = RotCW;
Speeds[4] = RotCCW;
Serial.print("JoyX = ");
Serial.print(JoystickX);
Serial.print(" LeftX = ");
Serial.print(LeftX);
Serial.print( " RightX = ");
Serial.print(RightX);
Serial.print( " LSpeed= ");
Serial.print (LTSpeed);
Serial.print( " RTSpeed= ");
Serial.print(RTSpeed);
Serial.print(FwdRev);
//Serial.print(RotCW);
//Serial.println(RotCCW);
}
void CamSwitch () {
if ((digitalRead(SwitchPos1) == LOW) && (digitalRead(SwitchPos2) == LOW)) {
//CamSwitcher.write(80);
PosPWM = 80;
Serial.print( "SAng=80 ");
}
if ((digitalRead(SwitchPos1) == HIGH) && (digitalRead(SwitchPos2) == LOW)) {
//CamSwitcher.write(45);
PosPWM = 45;
Serial.print( "SAng=45 ");
}
if ((digitalRead(SwitchPos1) == LOW) && (digitalRead(SwitchPos2) == HIGH)) {
// CamSwitcher.write(120);
PosPWM = 120;
Serial.print( " SAng=120 ");
}
Speeds[5] = PosPWM;
}
void ArmSend(){
controlBase = analogRead(A2);
mtrDegreeBase = map(controlBase, 0, 1024, 0, 180);
controlShoulder = analogRead(A3);
mtrDegreeShoulder = map(controlShoulder, 100, 570, 0, 180);
controlElbow = analogRead(A4);
mtrDegreeElbow = map(controlElbow, 170, 660, 0, 180);
controlWrist = analogRead(A5);
mtrDegreeWrist = map(controlWrist, 1023, 600, 0, 180);
controlRotate = analogRead(A6);
mtrDegreeRotate = map(controlRotate, 1023, 0, 0, 180);
Speeds[6] = mtrDegreeBase;
Speeds[7] = mtrDegreeShoulder;
Speeds[8] = mtrDegreeElbow;
Speeds[9] = mtrDegreeWrist;
Speeds[10] = mtrDegreeRotate;
Serial.print(" B =");
Serial.print(mtrDegreeBase);
Serial.print(" S =");
Serial.print(mtrDegreeShoulder);
Serial.print(" E =");
Serial.print(mtrDegreeElbow);
Serial.print(" W =");
Serial.print(mtrDegreeWrist);
Serial.print(" R =");
Serial.println(mtrDegreeRotate);
}
void JawSend() {
if (digitalRead(JawCloseButton) ==HIGH) { //When button pressed
JawPos = JawPos + 1;
//delay(15);
if (JawPos >=120) {
JawPos = 120; } // Go to function close jaws
Serial.print ( "Jaw Pos Angle = ");
Serial.println(JawPos);
Speeds[11] = JawPos;
//radio.write(&Speeds, sizeof(Speeds)); //Send the array
}
if (digitalRead(JawOpenButton)==HIGH) {
JawPos = JawPos - 1;
// delay(15);
if (JawPos <=0) {
JawPos = 0; } // When button pressed go to open jaws
Serial.print ( "Jaw Pos Angle = ");
Serial.println(JawPos);
Speeds[11] = JawPos;
//radio.write(&Speeds, sizeof(Speeds)); //Send the array
}
Speeds[11] = JawPos;
} Code for Receiver side:
/*
* Sketch to:
* V7 Adding jaws open and close
* V6 No real use
* V5A Cleaning up radio tx rx
* V5 Tweaking steering
* V4 Test to use joystick for rotate ccw cw
* Includes camera switching using pwm o/p on pin 2
* receive steering data
* Added rotating on crawler axis
* receive arm data
*/
//#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h> // include libraries
#include <Wire.h>
#include <Servo.h>
// Include Adafruit PWM Library
#include <Adafruit_PWMServoDriver.h>
#define MIN_PULSE_WIDTH 650
#define MAX_PULSE_WIDTH 2350
#define FREQUENCY 50
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Servo SwitchCam; // Initialise pwm for camera switching
// Define Motor Outputs on PCA9685 board
int motorBase = 0;
int motorShoulder = 1;
int motorElbow = 2;
int motorWrist = 3;
int motorRotate = 4;
int motorJaws = 5;
// Define Motor position variables
int mtrDegreeBase;
int mtrDegreeShoulder;
int mtrDegreeElbow;
int mtrDegreeWrist;
int mtrDegreeRotate;
int mtrDegreeJaws;
int pulse_wide;
int pulse_width;
int Speeds[12]; //Array size for receiving data
int RTSpeed; // Define variables
int LTSpeed;
int Rotate;
int FwdRev = 0;
int RotCW = 0;
int RotCCW = 0;
int i = 0;
int CamSwitcher[1];
int PosPwm;
//Motor A pins for driver card
int enA = 5;
int in1 = 7;
int in2 = 8;
//Motor B pins for driver card
int enB = 6;
int in3 = A0;
int in4 = A1;
RF24 radio(9, 10); // CE, CSN
const byte address[6] = "00001"; //set array which contains the addrress
void setup() {
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB, OUTPUT); //Set outputs to motor drivers
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
radio.begin(); //Initialise radio
//Setting radio parameters
radio.openReadingPipe(0,address);
radio.setPALevel(RF24_PA_LOW);
radio.setDataRate(RF24_250KBPS);
radio.setChannel(125);
radio.startListening();
Serial.begin(115200);
// Setup PWM Controller object
pwm.begin();
pwm.setPWMFreq(FREQUENCY);
SwitchCam.attach(2);
}
void loop() {
while (radio.available()) {
radio.read(&Speeds, sizeof(Speeds)); // Receive Speeds array
}
LTSpeed = Speeds[0]; // Assign variables to array contents
RTSpeed = Speeds[1];
FwdRev = Speeds[2];
RotCW = Speeds[3];
RotCCW = Speeds[4];
PosPwm = Speeds[5];
mtrDegreeBase = Speeds[6]; // Assign variables to array contents
mtrDegreeShoulder = Speeds[7];
mtrDegreeElbow = Speeds[8];
mtrDegreeWrist = Speeds[9];
mtrDegreeRotate = Speeds[10];
mtrDegreeJaws = Speeds[11];
JoyRx(); // function to receive speed/steering/spin data
CamSwitchRx(); // function to receive pwm value for cam switcher
ArmControlRx(); // function to receive arm control data
}
void JoyRx() {
// When joystick is fully one way sets motors to rotate CW
while (RotCW == HIGH) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, LTSpeed);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enB, RTSpeed);
Serial.print("LTSpeed = ");
Serial.print(LTSpeed);
Serial.print(" RTSpeed = ");
Serial.print(RTSpeed);
Serial.println (" Spin CW");
while (radio.available()) {
//Check if CW state is still applicable for while loop
radio.read(&Speeds, sizeof(Speeds));
}// Check button state
RotCW = Speeds[3];
LTSpeed = Speeds[0];
RTSpeed = Speeds[1];
}
//When joystick is fully the other way, set motors to rotate CCW
while (RotCCW == HIGH) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
analogWrite(enA, RTSpeed);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, LTSpeed);
Serial.print(" LTSpeed = ");
Serial.print(LTSpeed);
Serial.print(" RTSpeed = ");
Serial.print(RTSpeed);
Serial.println (" Spin CCW");
// Read radio to see if CCW is still valid
while (radio.available()) {
radio.read(&Speeds, sizeof(Speeds)); // Receive Speeds array
}
RotCCW = Speeds[4]; //Check button state
LTSpeed = Speeds[0];
RTSpeed = Speeds[1];
}
//For driving in reverse
if (FwdRev == LOW) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, RTSpeed);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, LTSpeed);
Serial.print( " Rev ");
Serial.print( " LTrack = " );
Serial.print(LTSpeed);
Serial.print( " RTrack = " );
Serial.print(RTSpeed);
}
//For driving forward
else {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
analogWrite(enA, LTSpeed);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enB, RTSpeed);
Serial.print( " Fwd ");
Serial.print( " LTrack = " );
Serial.print(LTSpeed);
Serial.print( " RTrack = " );
Serial.print(RTSpeed);
delay(3);
}
}
void CamSwitchRx() { //Function to receive PWM value
SwitchCam.write(PosPwm); //Write to outut pin for cam switcher
Serial.print( " CamPWM = ");
Serial.print(PosPwm);
delay(10);
}
void ArmControlRx() {
pulse_wide = map(mtrDegreeBase, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Base Motor
pwm.setPWM(motorBase, 0, pulse_width);
pulse_wide = map(mtrDegreeShoulder, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Shoulder Motor
pwm.setPWM(motorShoulder, 0, pulse_width);
pulse_wide = map(mtrDegreeElbow, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Elbow Motor
pwm.setPWM(motorElbow, 0, pulse_width);
pulse_wide = map(mtrDegreeWrist, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Wrist Motor
pwm.setPWM(motorWrist, 0, pulse_width);
pulse_wide = map(mtrDegreeRotate, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Rotate Motor
pwm.setPWM(motorRotate, 0, pulse_width);
pulse_wide = map(mtrDegreeJaws, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
//Control Jaws Motor
pwm.setPWM(motorJaws, 0, pulse_width);
Serial.print(" B =");
Serial.print(mtrDegreeBase);
Serial.print(" S =");
Serial.print(mtrDegreeShoulder);
Serial.print(" E =");
Serial.print(mtrDegreeElbow);
Serial.print(" W =");
Serial.print(mtrDegreeWrist);
Serial.print(" R =");
Serial.print(mtrDegreeRotate);
Serial.print(" J =");
Serial.println(mtrDegreeJaws);
}
The problem arises when I added this piece of code into the void() loop.
if ((digitalRead(JawCloseButton) ==HIGH)||(digitalRead(JawOpenButton) ==HIGH))
{
JawSend(); }
Even if a comment out the JawSend() function nor even add the JawSend value to the send array the receiver receives no data. It stops working with that couple of lines of code. I've even tried while loops, adding the code into the void ArmSend() function but nothing seems to work.
It is a really weird one, I have spent hours trying to figure out why adding that bit of code results in no data being received.
If anybody has any ideas, I would much appreciate it as it is really stumped me!
Thanks
Steve
