Im newish to the Arduino and certainly coding so please forgive me.
Ive been working on a walking robot (theo jansen style) driven by 2 x 18v drill motors. To control them I decided to write a VB.Net program which sends commands from my game pad over serial wirelessly (URF to XRF). I send back the received commands just so I can check its getting the instructions im sending. All of that is working fine and in fact I've had the robot working too, but after a little fiddling with the code late at night, I found that one of the motors no longer has speed control, its either on or off ?!?
I checked the mosfet swapped around the motors etc and found that all the hardware is fine. The gamepad send data also comes back as it should, so I also know that the arduino is sending and receiving what im telling it too.
So here's the code in full (disclaimer. this code is made up from bits and pieces I found on the net and a little of my own work, it probably isn't the best but it was working)
#include <avr/pgmspace.h>
#include <Servo.h>
int IRledPin = 3;
int motor1 = 10;
int motor2 = 11;
Servo XcamServo;
#define SOP '<'
#define EOP '>'
bool started = false;
bool ended = false;
char inData[20];
byte index;
void setup() {
pinMode(IRledPin, OUTPUT);
XcamServo.attach(9);
analogWrite(motor1, 0);
analogWrite(motor2, 0);
XcamServo.write(89);
Serial.begin(9600);
}
void loop() {
// Read all serial data available, as fast as possible
while (Serial.available() > 0) {
char inChar = Serial.read();
if (inChar == SOP) {
index = 0;
inData[index] = '\0';
started = true;
ended = false;
}
else if (inChar == EOP) {
ended = true;
break;
}
else {
if (index < 19) {
inData[index] = inChar;
index++;
inData[index] = '\0';
}
}
}
// We are here either because all pending serial
// data has been read OR because an end of
// packet marker arrived. Which is it?
if (started && ended) {
Serial.println(inData);
// The end of packet marker arrived. Process the packet
char *cmd = strtok(inData, ",");
if (cmd) {
char *val = strtok(NULL, ",");
char *val2 = strtok(NULL, ",");
if (val) {
sendCommand(cmd, val, val2);
}
}
// Reset for the next packet
started = false;
ended = false;
index = 0;
inData[index] = '\0';
}
}
void sendCommand(char *command, char *value, char *value2) {
if (strcmp(command,"Button") == 0) {
Button(value, value2);
}
if (strcmp(command,"LA") == 0) {
LA(value, value2);
}
if (strcmp(command,"RA") == 0) {
RA(value, value2);
}
}
void Button(char* value, char* value2) {
if (strcmp(value, "A") == 0) {
digitalWrite(IRledPin, HIGH);
}
if (strcmp(value, "B") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "X") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "Y") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "LB") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "RB") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "LS") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "RS") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "LT") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "RT") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "DUP") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "DDOWN") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "DLEFT") == 0) {
digitalWrite(IRledPin, LOW);
}
if (strcmp(value, "DRIGHT") == 0) {
digitalWrite(IRledPin, LOW);
}
}
void LA(char* value, char* value2) {
int x = atoi(value);
int y = atoi(value2);
int Lx = map(x,0, 2000, 0, 510);
int Ly = map(y, 1000, 2000, 0, 255);
if (Ly < 1){
analogWrite(motor1, 0);
analogWrite(motor2, 0);
}
else if (Ly > 0 && Lx == 255){
analogWrite(motor1, Ly);
analogWrite(motor2, Ly);
}
else if (Lx < 255){
int brakeFlip = map(Lx, 0, 254, Ly, 0);
int m1 = Ly - brakeFlip;
analogWrite(motor1, m1);
analogWrite(motor2, Ly);
}
else if (Lx > 255 ){
int brake = map(Lx, 256, 510, 0, Ly);
int m2 = Ly - brake;
analogWrite(motor1, Ly);
analogWrite(motor2, m2);
}
else {
analogWrite(motor1, Ly);
analogWrite(motor2, Ly);
}
}
void RA(char* value, char* value2) {
int xcam = atoi(value);
int ycam = atoi(value2);
int Rxcam = map(xcam,0, 2000, 0, 179);
if (Rxcam >90 || Rxcam <88){
XcamServo.write(Rxcam);
delay(15);
}
else{
XcamServo.write(89);
delay(15);
}
}
A lot of it can be ignored I expect, there's a bit of error checking etc all that seems fine. The issue comes down to the left analog stick where a bit of explanation is probably required.
When it comes to the Analog sticks the game pad sends out <LA,0000,000> the error checking looks for the <> to know the start and the end of the packet and disposes of them. so then you have LA,0000,0000. The LA means "Left analog" and the 0000,0000 are the X and Y. The Range of those No.s is 0-2000. Using X as an example, 0 = full left on the analog stick, 1000 =Centre & 2000 is far right.
Motors 1 & 2 are driven like a tank, so when you push the analog stick full forward both motors speed up to full speed. Then if you move the stick to the left or the right the X value is used as a break on the respective side. This is done using the map value & I guessing that's where my problem is but I just don't see it. Heres the code for the Left Analog Stick by itself
//Left Analog Stick X&Y
void LA(char* value, char* value2) {
//Change input chars to string
int x = atoi(value);
int y = atoi(value2);
//Map X axis
int Lx = map(x,0, 2000, 0, 510);
//Map Y axis
int Ly = map(y, 1000, 2000, 0, 255);
//if Y = 0, motor speed = 0
if (Ly < 1){
analogWrite(motor1, 0);
analogWrite(motor2, 0);
}
//if Y is larger than 0 and X is central, set motor speed to Ly (Y mapped from 0 - 255)
else if (Ly > 0 && Lx == 255){
analogWrite(motor1, Ly);
analogWrite(motor2, Ly);
}
//Heres the first Brake, if Lx = 0 -255
else if (Lx < 255){
//So here the Brake is being mapped and might seem a little confusing but I needed to make sure that I didn't have more brake than forward motion(Ly)
int brakeFlip = map(Lx, 0, 254, Ly, 0);
//m1 is the value of Ly(forward speed) - the Brake(breakflip)
int m1 = Ly - brakeFlip;
analogWrite(motor1, m1);
analogWrite(motor2, Ly);
}
//same for the opposite side brake
else if (Lx > 255 ){
int brake = map(Lx, 256, 510, 0, Ly);
int m2 = Ly - brake;
analogWrite(motor1, Ly);
analogWrite(motor2, m2);
}
else {
analogWrite(motor1, Ly);
analogWrite(motor2, Ly);
}
}
Its motor 1 that the problems with. It will only ever output at full (255), or off (0) even tho I can see that its receiving all its instructions. Motor 2 works fine.
Any help would be appreciated. Sorry for the long post, I wasn't sure how much detailed explanation would be needed heh.
Thanks in advance
