Supplies and Supply Arm

This code was used to control the robot to pick up the supplies with an arm controlled by a servo and deposit it in a drop zone.

#ifndef SUPPLIES_H
#define SUPPLIES_H

void pickupSupplies();
void dropOffSupplies();

#endif // SUPPLIES_H

// Required FEH libraries
#include <FEHIO.h>
#include <FEHLCD.h>

// Required custom libraries
#include "drive.h"
#include "supplyarm.h"
#include "start.h"
#include "constants.h"
#include "supplies.h"

/* This function makes the robot align with and pick up the supplies.
 * This function starts after the robot backs up from the blue toggle
 * and ends after aligning with the lower entrance if the temporary access ramp.
 * Last modified: 4/2/2016 JKL
 */
void pickupSupplies() {

    driveUntilTime(45, 90, 1450, true);
    driveUntilRPSyRange(11.9, 30, 0, 1.5, 4000); // CHECK THIS OUT!
    turnUntilTime(-70, 550);
    turnUntilRPS(0,20, 2000);

    driveUntilTime(90, 100, 600, false);
    driveUntilBump(90, 60, 2);

    driveUntilRPSy( RPSSuppliesY - RPSSuppliesYOffset, 27, 180, 8000);

    // Failsafe code
    // If the robot gets caught on the supply box and the bumpSide is not triggered,
    // drive away and retry alignment with the wall.
    if ( driveUntilBumpTimeout(90, 30, 2, 5000) == 1 ) {
        driveUntilTime( 0, 40, 500, true);
        driveUntilBump(90, 60, 2);
        driveUntilRPSy( RPSSuppliesY - RPSSuppliesYOffset, 27, 180, 8000);
        driveUntilBumpTimeout( 90, 30, 2, 2000);
    }

    lowerToPickupArm();
    raiseToPickupArm();

    driveUntilTime(90, 30, 200, true);
    driveUntilBump( 9,80,-2);

    driveUntilRPSy( RPSTempRampBottomY, 27, 270, 4000);
    turnUntilRPS(0, 20, 1000);
} // end pickupSupplies function

/* This function makes the robot drop off the supplies at the drop zone.
 * This function starts after a RPS heading check after the fuel button
 * and ends after driving away from the drop zone.
 * Last modified: 4/2/2016 JKL
 */
void dropOffSupplies() {

    driveUntilTime(88, 140, 950, false);
    driveUntilBump(90, 60, 2);

    // Drive with heading 180 until microswitch 6 is bumped
    while(microswitch6.Value()) {
        motor1.SetPercent(-20);
        motor2.SetPercent(20);
        motor3.SetPercent(20);
        motor4.SetPercent(-20);
    }
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    driveUntilTime(270, 50, 150, true);

    lowerToDepositArm();
    driveUntilTime(350, 80, 550, true);
    raiseToDepositArm();
} // end dropOffSupplies function

The basic robot control was provided by different functions under supplyarm.

#ifndef SUPPLYARM_H
#define SUPPLYARM_H

void initializeArm();
void lowerToPickupArm();
void raiseToPickupArm();
void lowerToDepositArm();
void raiseToDepositArm();

#endif // SUPPLYARM_H

/* This is part of a custom header that contains the functions to operate the servo
 * for the arm to pickup and deposit the supplies.
 * Last modified: 3/21/2016 JKL
 */

// Required FEH libraries
#include <FEHServo.h>
#include <FEHUtility.h>

// Required custom libraries
#include "constants.h"
#include "worldstate.h"
#include "supplyarm.h"

/* This function is run from void start() at the beginning of the run.
 * It sets the minimum and maximum of the servo and raises it to an upright position.
 * Last modified: 3/21/2016 JKL
 */
void initializeArm() {
    arm_servo.SetMin(SERVO_MIN);
    arm_servo.SetMax(SERVO_MAX);
    arm_servo.SetDegree(175);
}

/* This function is run to lower the arm to contact the supplies.
 * Last modified: 3/21/2016 JKL
 */
void lowerToPickupArm() {
    arm_servo.SetDegree(16);
    Sleep(1000);
}

/* This function is run to pickup the supplies without dropping it.
 * A delay must be added to ensure that the supplies are picked up.
 * Last modified: 3/21/2016 JKL
 */
void raiseToPickupArm() {
    for (int degree = 18; degree <= 175; degree+= 2 ) {
        arm_servo.SetDegree(degree);
        Sleep(10);
    }
}

/* This function is run to lower the supplies into the drop zone.
 * Last modified: 4/2/2016 JKL
 */
void lowerToDepositArm() {
    arm_servo.SetDegree(63);
    Sleep(400);
}

/* This function is run to raise the arm after the supplies are deposited.
 * Last modified: 3/21/2016 JKL
 */
void raiseToDepositArm() {
    arm_servo.SetDegree(160);
}