So im trying to write some code for my robot for school. I got my two motors set up to turn and move forward, however when I run the commands it does not stop. This is the code that was provided to us, so I am not familiar with it. There is more than this but this is essentially what gets it to move. Also, when I change the parameters of mtrLeft or mtrRight, nothing changes, so it seems like it cannot read distance and just never ends. Help!
void loadCommandQueue() {
cmdQueue.clear();
cmdQueue.add(VEHICLE_START_WAIT); // do not change this line - waits for start pushbutton
cmdQueue.add(VEHICLE_START); // do not change this line
// Define robot movement speeds
// Speed is encoder pulses per second.
// There is a maximum speed. Testing will be required to learn this speed
cmdQueue.add(VEHICLE_SET_MOVE_SPEED,800); // Speed used for forward movements
cmdQueue.add(VEHICLE_SET_TURN_SPEED,650); // Speed used for left or right turns
cmdQueue.add(VEHICLE_SET_ACCEL,600); // smaller is softer larger is quicker and less accurate moves
// Example list of robot movements
// This block is modified for each tournament
//cmdQueue.add(VEHICLE_FORWARD,4);
cmdQueue.add(VEHICLE_TURN_LEFT);
cmdQueue.add(VEHICLE_TURN_RIGHT);
/* cmdQueue.add(VEHICLE_FORWARD,10);
cmdQueue.add(VEHICLE_TURN_LEFT);
cmdQueue.add(VEHICLE_FORWARD,10);
cmdQueue.add(VEHICLE_TURN_LEFT);
cmdQueue.add(VEHICLE_FORWARD,10);
cmdQueue.add(VEHICLE_TURN_RIGHT);
cmdQueue.add(VEHICLE_FORWARD,5);
cmdQueue.add(VEHICLE_TURN_RIGHT);
cmdQueue.add(VEHICLE_FORWARD,425);
*/
// This MUST be the last command.
cmdQueue.add(VEHICLE_FINISHED);
}
void loop () {
case VEHICLE_FORWARD :
if (newCmd) {
distance = ((long) cmdQueue.getParameter1() * (long) ENCODER_COUNTS_PER_REV / (long) MM_PER_REV);
speed = speedFwd;
mtrLeft.startMove(distance,speed);
mtrRight.startMove(distance,speed);
setMotorOutputs();
}
if (mtrLeft.isStopped() && mtrRight.isStopped()) {
setMotorOutputs();
cmdQueue.next();
}
break;
case VEHICLE_TURN_RIGHT :
if (newCmd) {
distance = ENCODER_COUNTS_90_DEG;
speed = speedTurn;
mtrLeft.startMove(distance,speed);
mtrRight.startMove(distance,speed * -1);
setMotorOutputs();
}
}