Am I doing this write?

Hi,

First off thank you all for all the help you have given me in my noob questions it has really helped me out.

So I have made it so the Arduinos are speaking with each other Arduino Uno (Sender) Mega (Reciever). No I am just starting to write the code that will control the robot on the mega side. The reason I am on here is I want to see if I am doing this correct and it won’t be glitchy when trying to control the robot. Its basically a 3 omni wheeled robot that will have a servo the is controlled by a gyroscope in the controller.

The wheels are controlled by a joystick and the gyroscope (counter clockwise and clockwise motion).

I know it my have some errors and I am not hear to figure out why its not working I just would like to know if I am going in the right direction for smooth robot control.

Mega Code (One I am curious about)

#define FL1 22  //front wheel CW    
#define FL2 24  //front wheel CCW
#define FR1 26  //Firing Motor
#define FR2 28  //Firing Motor
#define BL1 30  //Back Left CW
#define BL2 32  //Back Left CCW
#define BR1 34  //Back Right CW
#define BR2 36  //Back Right CCW

//These pins below, should be PWM pins on arduino

#define pwm1 6
#define pwm2 7
#define pwm3 8
#define pwm4 9

int header;
int x, y, p, r, b;
int spacer;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial1.begin(9600);
  
  pinMode(FL1, OUTPUT);     
  pinMode(FL2, OUTPUT);     
  pinMode(BL1, OUTPUT);     
  pinMode(BL2, OUTPUT);     
  pinMode(BR1, OUTPUT);     
  pinMode(BR2, OUTPUT);     
  
  pinMode(pwm1, OUTPUT);
  pinMode(pwm2, OUTPUT);
  pinMode(pwm3, OUTPUT);
}

void loop() {
if (Serial1.available()){
 if (Serial1.read() == 255){
  while(Serial1.available() < 8)
  {}
  x = Serial1.read();delay(2);
  spacer = Serial1.read();delay(2);//Spacer so xbee doesnt read 43 X 3
  y = Serial1.read();delay(2);
  spacer = Serial1.read();delay(2);
  p = Serial1.read();delay(2);
  spacer = Serial1.read();delay(2);
  r = Serial1.read();delay(2);
  b = Serial1.read();delay(2);
  x = map(x,0,254,-255,255);//Easier to read for a PWM value
  y = map(y,0,254,-255,255);//""
 }
}


Serial.println("x, y, p, r, b");
Serial.print(y);
Serial.print(",");
Serial.print(x);
Serial.print(",");
Serial.print(p); //Pitch from gyroscope controls the Servo
Serial.print(",");
Serial.print(r); //Roll from gyroscop contorls the rotational motion
Serial.print(",");
Serial.println(b); //Counter for level of firing speed


motor_drive(x,y);
gyro_drive(r);
servo_control(p);
fire_speed(b);



delay(200);
}

void motor_drive(int side, int forward){
  //////Stop///////////
  while (side >= -25 && forward <= 25){
    
    analogWrite(pwm1,0);
    analogWrite(pwm2,0);
    analogWrite(pwm3,0);
    digitalWrite(FL1,LOW);digitalWrite(FL2,LOW);
    digitalWrite(BL1,LOW);digitalWrite(BL2,LOW);
    digitalWrite(BR1,LOW);digitalWrite(BR2,LOW);
    
  }
  ///////Forward////////
  while(side >= -25 && forward > 25){
    analogWrite(pwm1,0);analogWrite(pwm2,forward);analogWrite(pwm3,forward);
    digitalWrite(FL1,LOW);digitalWrite(BL1,HIGH);digitalWrite(BR1,HIGH);
    digitalWrite(FL2,LOW);digitalWrite(BL2,LOW);digitalWrite(BR2,LOW);
  }
 //////Backwards////////
 while(side >=-25 && forward < -10){
   forward = abs(forward);
    analogWrite(pwm1,0);analogWrite(pwm2,forward);analogWrite(pwm3,forward);
    digitalWrite(FL1,LOW);digitalWrite(BL1,LOW);digitalWrite(BR1,LOW);
    digitalWrite(FL2,LOW);digitalWrite(BL2,HIGH);digitalWrite(BR2,HIGH);
}

}

Uno (if it helps)

#include <SoftwareSerial.h>
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
#define PRINT_CALCULATED
#define PRINT_SPEED 300

SoftwareSerial Xbee(2,3);


const int VERT = 0; // analog
const int HORIZ = 1; // analog
const int SEL = 7; // digital
 int pwr = 13; //Power LED
float x;
float y;
float z;
float pitch, roll;
int p, r;
int counter = 40;
int buttonstate;
int prevbuttonstate;
void setup() {
  
Xbee.begin(9600);
Serial.begin(9600);

uint16_t status = dof.begin();
pinMode(SEL,INPUT);
pinMode(pwr,OUTPUT);
digitalWrite(SEL,HIGH);
 

}

void loop() {
digitalWrite(pwr,HIGH);
buttonstate = digitalRead(SEL);
int vertical, horizontal, select;  

//Counter for speed of firing motor
if (buttonstate != prevbuttonstate && buttonstate == 0){
 counter++;
}
if (counter > 3){
counter = 1;
}

prevbuttonstate = buttonstate;

//Calculate the Pitch and the roll  
dof.readAccel();
x = dof.calcAccel(dof.ax);
y = dof.calcAccel(dof.ay);
z = dof.calcAccel(dof.az);

pitch = atan2(x, sqrt(y * y) + (z * z));
roll = atan2(y, sqrt(x * x) + (z * z));
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
p = pitch;
r = roll;
p = map(p,-90,90,0,180);
r = map(r,-90,90,0,180);
p = p/10;
r = r/10;
p = round(p);
r = round(r);
p = p * 10;
r = r * 1

vertical = analogRead(VERT); // will be 0-1023
horizontal = analogRead(HORIZ); // will be 0-1023
select = digitalRead(SEL); // will be HIGH (1) if not pressed, and LOW (0) if pressed
vertical = map(vertical,0,1023,0,254);
horizontal = map(horizontal,0,1023,0,254);

Xbee.write(255);
Xbee.write(vertical);
Xbee.write(1);
Xbee.write(horizontal);
Xbee.write(1);
Xbee.write(p);
Xbee.write(1);
Xbee.write(r);
Xbee.write(counter);

Serial.print(vertical);
Serial.print(",");
Serial.print(horizontal);
Serial.print(",");
Serial.print(p);
Serial.print(",");
Serial.print(r);
Serial.print(",");
Serial.println(counter);
delay(PRINT_SPEED);
}

Sorry if its messy I tried to comment and clear things up. I know its not complete but I don’t want to get too far in to know I am going the wrong way.

Thank you,

-Scooter

delay(200);``#define PRINT_SPEED 300
Define "smooth" for us.

I think you need a digitalRight()

More seriously, the way you are receiving data will not allow smooth action with all those delay()s and a WHILE.

Use one of the examples from serial input basics. They all receive without blocking.

…R

Sorry the delays were so I could read the data on the serial monitor. I forgot to lower them when I posted this.