shoot.c

#pragma config(Sensor, in1,    lightSensor1,   sensorReflection)
#pragma config(Sensor, in2,    lightSensor2,   sensorReflection)
#pragma config(Sensor, dgtl1,  ultrasonic,     sensorSONAR_inch)
#pragma config(Sensor, dgtl3,  encoder,        sensorNone)
#pragma config(Sensor, dgtl8,  bump,           sensorNone)
#pragma config(Motor,  port1,           puncher,       tmotorNone, openLoop)
#pragma config(Motor,  port2,           leftDrive,     tmotorNone, openLoop)
#pragma config(Motor,  port3,           rightDrive,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           servo,         tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
 * Principles of Engineering
 *
 * Shooting Project
 *
 * Concept:
 * Drive until we are in the clear, scanning for boxes using the ultrasonic sensor
 * We'll drive back and forth until we see a clear spot to shoot from
 * Turn 90deg to face the scoring platform
 * Drive forward to the scoring platform
 * Based on how far we drove, we'll need to turn a specific amount, turn that amount
 *
 * Oh, and you'll want to invert one of the drive motors in Motor & Sensor Setup
 *
 * NOTE: I don't remember any of the variable names, so I'm guessing here. Play around with the values as well, as I obviously didn't have access to the robot
 *
 * Some intitutions to help you out:
 * 500 ticks is about the length of one field tile end to end
 *
 */

#define ULTRASONIC_THRESHOLD 40



void setChassis(int left, int right) {
    motor[leftDrive] = left;
    motor[rightDrive] = right;
}


void drive(int distance) {

    // Reset the quad encoder
    SensorValue[encoder] = 0;
    int error;

    // Drive loop
    do {
        setChassis(127 * sgn(distance), 127 * sgn(distance));
        error = distance - SensorValue[encoder];
    } while(abs(error) > 40);

    // Stop motors
    setChassis(0, 0);

}

void turn(int degrees) {
    int target = (float)degrees * 2.7;
    int error;

    do {
        setChassis(-40 * sgn(degrees), 40 * sgn(degrees));
        error = target - SensorValue[encoder];
    } while(abs(error) > 40);

}


int findShootingLocation(int direction) {
    SensorValue[encoder] = 0;

    while(SensorValue[encoder] < 2000 && SensorValue[ultrasonic] < ULTRASONIC_THRESHOLD) {
        setChassis(40, 40);
    }

    // If we didn't find a clear spot, we need to reverse
    if (SensorValue[encoder] > 2000) {
        return -1;
    }

    setChassis(0, 0);
    return SensorValue[encoder];

}


int faceTrashCan(int distance) {

    // Get angle in radians (see explanation sheet), and then convert to degrees
    int theta = (PI / 2) - atan( 2539 / (3385 - distance) ) * (180.0 / PI);
    turn(theta);
    
    return theta;
}


task main() {

    // Search for a spot to shoot, reversing our course if needed
    int distanceTraveled = 0;
    int direction = 30;
    do {
        distanceTraveled = findShootingLocation(direction);
        direction *= -1;
    } while(distanceTraveled < 0);

    // Now that we've found it, let's drive up to our shooting location (90 may need to be -90)
    turn(90);

    wait1Msec(1000);
    drive(800);

    // Now we need to figure out how far we need to turn to face the trash cans
    // I'm guessing that this is some proportion of how far we drove
    // If you think of a triangle, we're making a hypotenuse with our shot,
    // and we need to figure out how far to turn to make that triangle make sense
    faceTrashCan(distanceTraveled);


    // Fire the ping pong ball, and hope for the best
    motor[puncher] = 127;
}