7686C Program IV

#include <cmath>

using namespace vex;

#include "robot-config.h"

//Creates a competition object that allows access to Competition methods.
competition Competition;

void sleep(double s){
    task::sleep(s * 1000);
}

enum DrivingMode { speed, precision };
enum AimColor { red, blue };

void run(motor *Motor, double speed){
    if(speed != 0){
        Motor->spin(directionType::fwd, speed, velocityUnits::rpm);
    } else {
        Motor->stop(brakeType::coast);
    }
}

class Robot;

class VisionTracker {
    public:
    AimColor *aimColor;
    vision::signature *blueflag, *redflag;
    vision *VisionSensor;
    long framesSinceFlag;
    int screen_x_middle, current_x_middle, screen_y_middle, current_y_middle;

    VisionTracker(AimColor *robotAimColor){
        aimColor = robotAimColor;

        blueflag = new vision::signature(2, -4229, -2759, -3494, 7523, 14313, 10918, 1.399999976158142, 0);
        redflag = new vision::signature(1, 6843, 8403, 7622, -421, 395, -14, 3.9, 0);
        VisionSensor = new vision(PORT7 /* 19 */, 50, *blueflag, *redflag);
        framesSinceFlag = 0;

        screen_x_middle = 316 / 2;
        screen_y_middle = 212 / 2;
    }

    void calculate(){
        if(*aimColor == AimColor::blue){
            VisionSensor->takeSnapshot(*blueflag);
        } else if(*aimColor == AimColor::red){
            VisionSensor->takeSnapshot(*redflag);
        }

        if(VisionSensor->objectCount > 0){
            current_x_middle = VisionSensor->largestObject.centerX;
            current_y_middle = VisionSensor->largestObject.centerY;
            framesSinceFlag = 0;
        } else {
            framesSinceFlag++;
        }
    }

    void display(controller *Con, int row){
        Con->Screen.clearLine(3);
        Con->Screen.setCursor(3, 1);
        if(framesSinceFlag < 5){
            Con->Screen.print("[%c] Y: %3d, X: %3d", *aimColor == AimColor::blue ?'B':'R', getY(), getX());
            if(abs(getX()) < 8) Con->rumble("-");
        } else {
            Con->Screen.print("[%c] No flag", *aimColor == AimColor::blue ?'B':'R');
        }
    }

    int getX(){
        return screen_x_middle - current_x_middle;
    }

    int getY(){
        return screen_y_middle - current_y_middle;
    }
};

const double autoSpeed = 125;

class Robot {
    public:
    brain *Brain;
    controller *Con1;
    motor *DriveL, *DriveR, *FlyWheelL, *FlyWheelR, *Feedbelt, *Kicker, *Intake;
    VisionTracker *visionTracker;

    bool flywheel_running;
    double flywheel_target_speed;
    bool intake_running;
    DrivingMode driving_mode;
    AimColor aim_color;

    Robot(){
        Brain = new brain();
        Con1 = new controller(controllerType::primary);

        DriveL = new motor(PORT1, gearSetting::ratio18_1, false);
        DriveR = new motor(PORT10, gearSetting::ratio18_1, true);
        FlyWheelL = new motor(PORT4 /* 12 */, gearSetting::ratio18_1, false);
        FlyWheelR = new motor(PORT6 /* 20 */ , gearSetting::ratio18_1, true);
        Feedbelt = new motor(PORT5 /* 13 */, false); // Sharpie
        Kicker = new motor(PORT9, true);
        Intake = new motor(PORT11, false);

        DriveL->setStopping(brakeType::coast);
        DriveR->setStopping(brakeType::coast);

        flywheel_running = false;
        flywheel_target_speed = 70;
        intake_running = false;
        driving_mode = DrivingMode::speed;
        aim_color = AimColor::red;

        visionTracker = new VisionTracker(&aim_color);
    }

    double calc_rpm(double in){
        return in / 127.0 * 200.0;
    }

    void toggle_aim_color(){
        aim_color = aim_color == AimColor::blue ? AimColor::red : AimColor::blue;
    }

    void toggle_driving_mode(){
        driving_mode = driving_mode == DrivingMode::speed ? DrivingMode::precision : DrivingMode::speed;
    }

    void runDrive(double l, double r){
        run(DriveL, l);
        run(DriveR, r);
    }

    void runDrive(double v){
        runDrive(v,v);
    }

    void runFly(double speed){
        run(FlyWheelL, speed);
        run(FlyWheelR, speed);
    }

    void runBelt(double speed){
        run(Feedbelt, speed);
    }

    void moveKicker(bool up){
        if(up){
            Kicker->rotateTo(-0.3, rotationUnits::rev, 200, velocityUnits::rpm, false);
        } else {
            Kicker->rotateTo(0, rotationUnits::rev, 200, velocityUnits::rpm, false);
        }
    }

    void driveDistance(double distance_l, double distance_r, distanceUnits units, bool block){
        double revolutions_multiplier = 0;

        if(units == distanceUnits::in){ // Inches
            revolutions_multiplier = 1.0 / (4.0 * M_PI);
        } else if(units == distanceUnits::cm){ // Centimeters
            revolutions_multiplier = 1.0 / (4.0 * 2.54 * M_PI);
        } else if(units == distanceUnits::mm){ // Millimeters
            revolutions_multiplier = 1.0 / (4.0 * 25.4 * M_PI);
        }

        DriveL->rotateFor(distance_l * revolutions_multiplier, rotationUnits::rev, autoSpeed, velocityUnits::rpm, false);
        DriveR->rotateFor(distance_r * revolutions_multiplier, rotationUnits::rev, autoSpeed, velocityUnits::rpm, block);
    }

    void driveDistance(double distance, distanceUnits units, bool block){
        driveDistance(distance, distance, units, block);
    }

    void driveAngle(double angle, bool block){
        double distance_multiplier = 2 * M_PI * 5.9; // 5.9 inches is the radius of the turn circle
        driveDistance(distance_multiplier * angle, -distance_multiplier * angle, distanceUnits::in, block);
    }

    // drive distance rotation is 5.9 inches

    void preautonomous();
    void autonomous();
    void predrive();
    void drive();
    void feedback(long ticks);
};

void Robot::preautonomous(){
    Brain->Screen.setFont(fontType::mono60);
}

void Robot::autonomous(){
    //driveAngle(0.25, true);
    /*
    driveDistance(21, distanceUnits::in, true);
    runFly(65);
    sleep(3.0);
    runBelt(-150);
    sleep(1.0);
    runFly(0);
    runBelt(0);

    runDrive(175, 200);
    sleep(0.75);
    runDrive(0,0);

    driveDistance(-25, distanceUnits::in, true);

    driveAngle( -65.0 / 360.0, true);
    //runDrive(100, -100);
    //sleep(0.50);
    //runDrive(0);

    sleep(0.1);// wait
    runDrive(100);
    sleep(1.0);
    runDrive(0);
    sleep(0.1); // wait
    moveKicker(true);
    sleep(0.5);
    moveKicker(false);
    */


    // Other programs
    /*MotorL.rotateFor(36/12.57, rotationUnits::rev, 150, velocityUnits::rpm, false);
    MotorR.rotateFor(36/12.57, rotationUnits::rev, 150, velocityUnits::rpm, true);
    MotorL.rotateFor(-12/12.57, rotationUnits::rev, 150, velocityUnits::rpm, false);
    MotorR.rotateFor(-12/12.57, rotationUnits::rev, 150, velocityUnits::rpm, true);*/
    /*run_fly(70);
    sleep(5.0);
    run_belt(-150);
    sleep(1.0);
    run_fly(0);
    run_belt(0);*/


    driveDistance(40, distanceUnits::in, true);
    driveDistance(-30, distanceUnits::in, true);
    driveAngle(50.0 / 360.0, true);
    driveDistance(30, distanceUnits::in, true);
    moveKicker(true);
    sleep(0.5);
    moveKicker(false);
    driveDistance(-10, distanceUnits::in, true);

}

void Robot::predrive(){
    // pass
}

void Robot::drive(){
    if(driving_mode == DrivingMode::precision){
        double baseSpeed = calc_rpm(Con1->Axis3.value() / 4.0);
        double bias = Con1->Axis4.value() / 4.0;
        runDrive(baseSpeed + bias, baseSpeed - bias);
    } else {
        double left = calc_rpm(Con1->Axis3.value());
        double right = calc_rpm(Con1->Axis2.value());
        double sensitivity = ((left * right) < 1) ? 0.5 : 1.0;

        runDrive(left * sensitivity, right * sensitivity);
    }

    moveKicker(Con1->ButtonR1.pressing());
    runBelt((Con1->ButtonL1.pressing()*150) + (Con1->ButtonL2.pressing()*-150));

    if(Con1->ButtonR2.pressing()){
        run(Intake, -200);
    } else if(Con1->ButtonL2.pressing() || !intake_running){
        run(Intake, 0);
    } else {
        run(Intake, 200);
    }

    runFly(flywheel_running ? flywheel_target_speed : 0);
}

void Robot::feedback(long ticks){
    // Shows which color we are aiming for and flashes yellow if in precision driving mode.
    if(driving_mode == DrivingMode::precision && ticks % 2 == 0){
        Brain->Screen.clearScreen("#FF0");
    } else if(aim_color == AimColor::blue){
        Brain->Screen.clearScreen("#00F");
    } else {
        Brain->Screen.clearScreen("#F00");
    }

    Con1->Screen.clearLine(2);
    Con1->Screen.setCursor(2,1);
    if(driving_mode == DrivingMode::precision){
        Con1->Screen.print("Precision");
    } else {
        Con1->Screen.print("Speed");
    }

    double average_speed = (FlyWheelL->velocity(velocityUnits::rpm) + FlyWheelR->velocity(velocityUnits::rpm)) / 2;
    Con1->Screen.clearLine(1);
    Con1->Screen.setCursor(1, 1);
    Con1->Screen.print("T: %3.0f, M: %3.0f", flywheel_target_speed, average_speed); // T for target, M for measured
}

Robot *robot = new Robot();

void do_360(){
    robot->runFly(70);
    robot->driveAngle(0.98, true);
    robot->runBelt(-150);
    sleep(1.0);
    robot->runBelt(0);
    robot->runFly(0);
}

void autonomous(){ robot->autonomous(); }

void drive(){
    // TODO: Fix issues with scope (robot pointer in lambda) [std::bind]
    robot->Con1->ButtonA.pressed([]{ robot->flywheel_running = !robot->flywheel_running; });
    robot->Con1->ButtonY.pressed([]{ robot->toggle_driving_mode(); });
    robot->Con1->ButtonUp.pressed([]{ robot->flywheel_target_speed += 10; });
    robot->Con1->ButtonLeft.pressed([]{ robot->flywheel_target_speed -= 10; });
    robot->Con1->ButtonX.pressed([]{ robot->toggle_aim_color(); });
    robot->Con1->ButtonRight.pressed([]{ robot->intake_running = !robot->intake_running; });

    long ticks = 0;
    while (1) {
        robot->drive();

        if(robot->Con1->ButtonB.pressing()){
            do_360();
        }

        if(ticks % 5 == 0){
            robot->feedback(ticks / 5);
            robot->visionTracker->calculate();
            robot->visionTracker->display(robot->Con1, 3);
        }

        sleep(1.0 / 50.0);

        ticks++;
    }
}

int main() {
    robot->preautonomous();
    robot->Brain->Screen.printAt(20,50,"Preauto");

    Competition.autonomous(autonomous);
    Competition.drivercontrol(drive);

    while(1){
        sleep(1);
    }
}