+/*************************************

+ Drive: mid level movement functions

+*************************************/

+

+int moveState = MOV_STOP; // what robot is doing

+

+int moveSpeed = 0; // move speed stored here (0-100%)

+int speedIncrement = 10; // percent to increase or decrease speed

+

+void moveBegin()

+{

+ motorBegin(MOTOR_LEFT);

+ motorBegin(MOTOR_RIGHT);

+ moveStop();

+}

+

+void moveLeft()

+{

+ changeMoveState(MOV_LEFT);

+ motorForward(MOTOR_LEFT, 0);

+ motorForward(MOTOR_RIGHT, moveSpeed);

+ Serial.println("were going left... maybe????");

+}

+

+void moveRight()

+{

+ changeMoveState(MOV_RIGHT);

+ motorForward(MOTOR_LEFT, moveSpeed);

+ motorForward(MOTOR_RIGHT, 0);

+ Serial.println("were going right... maybe????");

+}

+

+void moveForward()

+{

+ changeMoveState(MOV_FORWARD);

+ motorForward(MOTOR_LEFT, moveSpeed);

+ motorForward(MOTOR_RIGHT, moveSpeed);

+}

+

+void moveBackward()

+{

+ changeMoveState(MOV_BACK);

+ motorReverse(MOTOR_LEFT, moveSpeed);

+ motorReverse(MOTOR_RIGHT, moveSpeed);

+}

+

+void moveRotate(int angle)

+{

+ changeMoveState(MOV_ROTATE);

+ Serial.print("Rotating "); Serial.print(angle);

+ if(angle < 0)

+ {

+ Serial.println(" (left)");

+ motorReverse(MOTOR_LEFT, moveSpeed);

+ motorForward(MOTOR_RIGHT, moveSpeed);

+ angle = -angle;

+ }

+ else if(angle > 0)

+ {

+ Serial.println(" (right)");

+ motorForward(MOTOR_LEFT, moveSpeed);

+ motorReverse(MOTOR_RIGHT, moveSpeed);

+ }

+ int ms = rotationAngleToTime(angle, moveSpeed);

+ movingDelay(ms);

+ moveBrake();

+}

+

+void moveStop()

+{

+ changeMoveState(MOV_STOP);

+ motorStop(MOTOR_LEFT);

+ motorStop(MOTOR_RIGHT);

+ Serial.println("stop lmao");

+}

+

+void moveBrake()

+{

+ changeMoveState(MOV_STOP);

+ motorBrake(MOTOR_LEFT);

+ motorBrake(MOTOR_RIGHT);

+}

+

+void moveSetSpeed(int speed)

+{

+ motorSetSpeed(MOTOR_LEFT, speed) ;

+ motorSetSpeed(MOTOR_RIGHT, speed) ;

+ moveSpeed = speed; // save the value

+}

+

+void moveSlower(int decrement)

+{

+ Serial.print(" Slower: ");

+ if( moveSpeed >= speedIncrement + MIN_SPEED)

+ moveSpeed -= speedIncrement;

+ else moveSpeed = MIN_SPEED;

+ moveSetSpeed(moveSpeed);

+}

+

+

+void moveFaster(int increment)

+{

+ Serial.print(" Faster: ");

+ moveSpeed += speedIncrement;

+ if(moveSpeed > 100)

+ moveSpeed = 100;

+ moveSetSpeed(moveSpeed);

+}

+

+int moveGetState()

+{

+ return moveState;

+}

+

+/*************************************

+ functions to rotate the robot

+*************************************/

+

+// return the time in milliseconds to turn the given angle at the given speed

+long rotationAngleToTime( int angle, int speed)

+{

+int fullRotationTime; // time to rotate 360 degrees at given speed

+

+ if(speed < MIN_SPEED)

+ return 0; // ignore speeds slower then the first table entry

+

+ angle = abs(angle);

+

+ if(speed >= 100)

+ fullRotationTime = rotationTime[NBR_SPEEDS-1]; // the last entry is 100%

+ else

+ {

+ int index = (speed - MIN_SPEED) / SPEED_TABLE_INTERVAL ; // index into speed and time tables

+ int t0 = rotationTime[index];

+ int t1 = rotationTime[index+1]; // time of the next higher speed

+ fullRotationTime = map(speed, speedTable[index], speedTable[index+1], t0, t1);

+ // Serial.print("index= "); Serial.print(index); Serial.print(", t0 = "); Serial.print(t0); Serial.print(", t1 = "); Serial.print(t1);

+ }

+ // Serial.print(" full rotation time = "); Serial.println(fullRotationTime);

+ long result = map(angle, 0,360, 0, fullRotationTime);

+ return result;

+}

+

+

+// rotate the robot from MIN_SPEED to 100% increasing by SPEED_TABLE_INTERVAL

+void calibrateRotationRate(int direction, int angle)

+{

+ Serial.print(locationString[direction]);

+ Serial.println(" calibration" );

+ for(int speed = MIN_SPEED; speed <= 100; speed += SPEED_TABLE_INTERVAL)

+ {

+

+ delay(1000);

+ //blinkNumber(speed/10);

+

+ if( direction == DIR_LEFT)

+ { // rotate left

+ motorReverse(MOTOR_LEFT, speed);

+ motorForward(MOTOR_RIGHT, speed);

+ }

+ else if( direction == DIR_RIGHT)

+ { // rotate right

+ motorForward(MOTOR_LEFT, speed);

+ motorReverse(MOTOR_RIGHT, speed);

+ }

+ else

+ Serial.println("Invalid direction");

+

+

+ int time = rotationAngleToTime(angle, speed);

+

+ Serial.print(locationString[direction]);

+ Serial.print(": rotate ");

+ Serial.print(angle);

+ Serial.print(" degrees at speed ");

+ Serial.print(speed);

+ Serial.print(" for ");

+ Serial.print(time);

+ Serial.println("ms");

+ delay(time);

+ motorStop(MOTOR_LEFT);

+ motorStop(MOTOR_RIGHT);

+ delay(2000); // two second delay between speeds

+ }

+}

+

+// this is the low level movement state.

+// it will differ from the command state when the robot is avoiding obstacles

+void changeMoveState(int newState)

+{

+ if(newState != moveState)

+ {

+ Serial.print("Changing move state from "); Serial.print( states[moveState]);

+ Serial.print(" to "); Serial.println(states[newState]);

+ moveState = newState;

+ }

+}

+

+/************* high level movement functions ****************/

+

+//moves in the given direction at the current speed for the given duration in milliseconds

+void timedMove(int direction, int duration)

+{

+ Serial.print("Timed move ");

+ if(direction == MOV_FORWARD) {

+ Serial.println("forward");

+ moveForward();

+ }

+ else if(direction == MOV_BACK) {

+ Serial.println("back");

+ moveBackward();

+ }

+ else

+ Serial.println("?");

+

+ movingDelay(duration);

+ moveStop();

+}

+

+// check for obstacles while delaying the given duration in ms

+

+void movingDelay(long duration)

+{

+ long startTime = millis();

+ while(millis() - startTime < duration) {

+ // function in Look module checks for obstacle in direction of movement

+ if(checkMovement() == false) {

+ if( moveState != MOV_ROTATE) // rotate is only valid movement

+ {

+ Serial.println("Stopping in moving Delay()");

+ moveBrake();

+ }

+ }

+ }

+}

+

+boolean checkMovement() {

+ return true;

+}

+

+