I am writing a script that allows me to order motors to move a set distance. I want it to be able to respond to multiple orders at a time without needing to reset the arduino. Here's the code:
#include <SoftwareSerial.h>
#include "DualTB9051FTGMotorShieldMod3230.h"
#include "Encoder.h"
// Declaring remapping for motor 1
unsigned char M1EN = 22;
unsigned char M1DIR = 7;
unsigned char M1PWM = 9;
unsigned char M1DIAG = 6;
unsigned char M1OCM = A0;
// Declaring remapping for motor 2
unsigned char M2EN = 4;
unsigned char M2DIR = 8;
unsigned char M2PWM = 10;
unsigned char M2DIAG = 12;
unsigned char M2OCM = A1;
// Declaring remapping for motor 3
unsigned char M3EN = 23;
unsigned char M3DIR = 27;
unsigned char M3PWM = 45;
unsigned char M3DIAG = 31;
unsigned char M3OCM = A2;
// Just in case setup for motor 4
unsigned char M4EN = 25;
unsigned char M4DIR = 29;
unsigned char M4PWM = 46;
unsigned char M4DIAG = 33;
unsigned char M4OCM = A4;
// Command to perform remaps
DualTB9051FTGMotorShieldMod3230 md(M1EN,M1DIR,M1PWM,M1DIAG,M1OCM,M2EN,M2DIR,M2PWM,
M2DIAG,M2OCM,M3EN,M3DIR,M3PWM,M3DIAG,M3OCM,M4EN,
M4DIR,M4PWM,M4DIAG,M4OCM);
// Declaring encoders
Encoder M1Encoder(2,3);
Encoder M2Encoder(18,19);
Encoder M3Encoder(20,21);
// Orders variables
String inString; // Storage variable for receiving orders
float angleOrder; // Angle the arm will rotate to
float extOrder; // Distance the arm will extend to reach the dock
float completeTime; // Time that the operation will be completed in
// Hardware Constants
float motorGearRatio = 131; // 131 RevsMotor/RevsShaft
float countsPerMotorRev = 64; // 64 counts/RevsMotor
float grabberGearRadius = .85; // Radius of grabber gear in inches
float innerGearRadius = .54; // Radius of inner gear in inches
float leverArm = 33.5; // Distance of drive wheel from base in inches
float wheelRadius = 1.37795276; // Wheel radius in inches
float kp2 = 150; // Tuning variable for function
float kp3 = 80;
// Position tracking variables
float counts = 0; // Counts
float pos2 = 0; // Current encoder reading for motor 2
float pos3 = 0; // Current encoder reading for motor 3
float startTime = 0; // Time the operation actually starts
float t2 = 0; // Current time
float tOld2 = 0; // Previous time
float t3 = 0;
float tOld3 = 0;
float deltaT = 0; // Difference between t & tOld
float shaftPos = 0; // Current motor shaft position in radians
float armPos = 0; // Actual current arm position in degrees
float armExt = 0; // Actual current arm extension in inches
float M2 = 0; // Motor 2 power
float M3 = 0; // Motor 3 power
float omegaDesired; // Desired rotational velocity
float velDesired; // Desired extension velocity
float extD; // Iteration desired extension variable
float angleD; // Iteration desired angle variable
void setup() {
// Initializing communication
Serial.begin(115200); //Open serial communications with computer and wait for port to open:
Serial2.begin(9600); //Open serial communications with the other Arduino board
//Initializing motors
md.init();
md.enableDrivers();
}
void loop() {
// Pull orders from Uno **********************************************************************
if(Serial2.available() > 4){ //If the XBee has 5 or more bytes to read
if(Serial2.read() != 255){ // If the first byte isn't 255
// Flush the serial monitor
while(Serial2.available() != 0){
Serial2.read();
}
}
else{
// Pull angle orders
inString = Serial2.readStringUntil(',');
Serial2.read();
angleOrder = inString.toFloat();
// DEBUGGING SERIAL PRINTS ------------------------------------------
Serial.print("Arm will move to position ");
Serial.print(angleOrder);
Serial.println(" degrees");
// DEBUGGING SERIAL PRINTS ------------------------------------------
// Pull extension orders
inString = Serial2.readStringUntil(',');
Serial2.read();
extOrder = inString.toFloat();
// DEBUGGING SERIAL PRINTS ------------------------------------------
Serial.print("Arm will extend ");
Serial.print(extOrder);
Serial.println(" inches");
// DEBUGGING SERIAL PRINTS ------------------------------------------
// Pull time orders
inString = Serial2.readStringUntil(',');
Serial2.read();
completeTime = inString.toFloat();
// DEBUGGING SERIAL PRINTS ------------------------------------------
Serial.print("Operation will finish in ");
Serial.print(completeTime);
Serial.println(" seconds");
// DEBUGGING SERIAL PRINTS ------------------------------------------
completeTime = completeTime * 1000.0;
// Flush out any remaining data in the serial buffer
while(Serial2.available() != 0){
Serial2.read();
}
}
// desired velocities
velDesired = extOrder / completeTime;
omegaDesired = (angleOrder - armPos) / completeTime;
// recording the starting times
tOld2 = millis();
tOld3 = millis();
startTime = millis();
// DEBUGGING SERIALPRINTS ------------------------------
// Serial.print("The operation start time is: ");
// Serial.println(startTime);
// Serial.print("The desired exention velocity in in/ms is: ");
// Serial.println(velDesired, 15);
// DEBUGGING SERIALPRINTS ------------------------------
}
// Execute orders **********************************************************************************
// Run motor 2
t2 = millis();
pos2 = M2Encoder.read();
//velDesired = (extOrder - armExt) / (completeTime - (t2 - startTime)); // In inches/millisecond
shaftPos = -1 * ((pos2 / (countsPerMotorRev * motorGearRatio)) * 2.0*PI); // Position of the shaft in radians
deltaT = t2 - tOld2; // Time difference between running the function in milliseconds
tOld2 = t2; // Updating the time
armExt = shaftPos * innerGearRadius; // Get the current extension of the arm in inches
// Proportion control
if(abs(extOrder) > abs(armExt)){
extD = extD + velDesired * deltaT;
}
else{
extD = extOrder;
}
if(abs(extOrder) < abs(armExt)){
Serial.println("getting in here");
extOrder = 0;
armExt = 0;
shaftPos = 0;
}
M2 = kp2*(extD - armExt);
md.setM2Speed(-M2);
// DEBUGGING SERIAL PRINTS ------------------------------------------------------
Serial.print("Current Arm extension: ");
Serial.println(armExt);
// Serial.print("The time difference is: ");
// Serial.println(deltaT);
// Serial.print("Desired velocity: ");
// Serial.println(velDesired);
// Serial.print("Desired extension value: ");
// Serial.println(extD);
// Serial.print("Motor power: ");
// Serial.println(M2);
// DEBUGGING SERIAL PRINTS ------------------------------------------------------
// Run motor 3
t3 = millis();
pos3 = M3Encoder.read();
shaftPos = (pos3 / (countsPerMotorRev * motorGearRatio)) * 2.0*PI; // Position of the shaft in radians
deltaT = t3 - tOld3; // Time difference between running the function in milliseconds
tOld3 = t3; // Updating the time // Updating the position
armPos = shaftPos * (wheelRadius / leverArm) * (180 / PI); // Get the current position of the arm in degrees
// Proportion control
if((omegaDesired > 0 && angleOrder > armPos) || (omegaDesired < 0 && angleOrder < armPos)){
angleD = angleD + omegaDesired * deltaT;
}
else{
angleD = angleOrder;
}
M3 = kp3*(angleD - armPos);
md.setM3Speed(M3);
// DEBUGGING SERIAL PRINTS ------------------------------------------------------
// Serial.print("Current Arm Pos: ");
// Serial.println(armPos);
// Serial.print("The time difference is: ");
// Serial.println(deltaT);
// Serial.print("Iteration desired angle value: ");
// Serial.println(angleD);
// Serial.print("Motor power: ");
// Serial.println(M3);
// Serial.print("Ideration desired angle: ");
// DEBUGGING SERIAL PRINTS ------------------------------------------------------
}
The problem is here:
if(abs(extOrder) < abs(armExt)){
Serial.println("getting in here");
extOrder = 0;
armExt = 0;
shaftPos = 0;
}
When I first upload the code, it doesn't get inside the if statement, but after I have had it execute its first orders, it gets inside the if statement once, then continues to enter it with each loop through the code. I don't understand why or how to prevent it.