Tustin Code

Lesson 22 - Example Robot Code

Example Robot Code 

Below is some code that is meant to be used as an example for what a competition file could be for a lift robot with a claw on it. By no means is this the best code, but if you are just starting off, writing something like this is a great accomplishment. If you are more advanced, we highly recommend you add more complexity to the autonomous movement functions (such as using PID instead) and innovate on the drive code (such as potentiometer lift preset positions).


#include "main.h"

pros::Controller controller(CONTROLLER_MASTER);
pros::Motor rDriveF(3, MOTOR_GEARSET_18, false, MOTOR_ENCODER_DEGREES);
pros::Motor rDriveB(11, MOTOR_GEARSET_18, false, MOTOR_ENCODER_DEGREES);
pros::Motor lDriveF(4, MOTOR_GEARSET_18, true, MOTOR_ENCODER_DEGREES);
pros::Motor lDriveB(14, MOTOR_GEARSET_18, true, MOTOR_ENCODER_DEGREES);
pros::Motor claw(13, MOTOR_GEARSET_36, false, MOTOR_ENCODER_DEGREES);
pros::Motor leftLift(1, MOTOR_GEARSET_18, false, MOTOR_ENCODER_DEGREES);
pros::Motor rightLift(12, MOTOR_GEARSET_18, true, MOTOR_ENCODER_DEGREES);
pros::Imu imu(15);

void initialize() {
}
void disabled() {}

void competition_initialize() {}

void chas_move(float lspeed, float rspeed){
    lDriveF.move(lspeed);
    lDriveB.move(lspeed);
    rDriveF.move(rspeed);
    rDriveB.move(rspeed);
}

void reset_encoders(){
    lDriveF.tare_position();
    lDriveB.tare_position();
    rDriveF.tare_position();
    rDriveB.tare_position();
}

void driveFor(int target, int speed){ //make target and speed negative for backwards movement
    float encoder_av;
    int direction;
    reset_encoders();
    chas_move(speed, speed); //sets chassis to the directed speed
    direction = target/abs(target); //finds out if the robot is going forward or backwards
    while(true){
        encoder_av = (lDriveB.get_position() + rDriveB.get_position())/2; //finds robot position
        if(abs(encoder_av) > abs(target)){ //stops once robot reaches target
            chas_move(0, 0);
            break;
        }
    }
    while(abs(target - encoder_av) > 10){ //drives in the opposite direction to correct some error
        encoder_av = (lDriveB.get_position() + rDriveB.get_position())/2;
        chas_move(10 * direction * -1, 10 * direction * -1);
    }
    chas_move(0, 0);
}

void turnFor(int target, int speed){ //make target and speed negative for clockwise movement
    int imu_start = imu.get_rotation();
    float position;
    chas_move(-speed, speed); //sets chassis to the directed speed
    while(true){
        position = imu.get_rotation() - imu_start;
        if (abs(target - position) <= 1.5){ //stops once robot reaches target
            chas_move(0, 0);
            break;
        }
    }
}

void autonomous() {
    driveFor(1000, 100);
    turnFor(120, 70);
    claw.move(127);
}

void opcontrol() {
    while (true) {
       
        //lift code
        if(controller.get_digital(DIGITAL_L1)){
            leftLift.move(127);
            rightLift.move(127);
        }
        else if(controller.get_digital(DIGITAL_L2)){
            leftLift.move(-127);
            rightLift.move(-127);
        }
        else{
            leftLift.move(0);
            rightLift.move(0);
        }

        //claw code
        if(controller.get_digital(DIGITAL_R1)){
            claw.move(127);
        }
        else if(controller.get_digital(DIGITAL_R2)){
            claw.move(-127);
        }
        else{
            claw.move(0);
        }

        //arcade drive code
        int fPwr, tPwr; //forwardpower and turnpower
        int rYaxis, lXaxis; //controller axis
        rYaxis = controller.get_analog(ANALOG_RIGHT_Y);
        lXaxis = controller.get_analog(ANALOG_LEFT_X);
        fPwr = (abs(rYaxis) > 4) ? rYaxis : 0;
        tPwr = (abs(lXaxis) > 4) ? lXaxis : 0; 
        chas_move(fPwr + tPwr, fPwr - tPwr);

        pros::delay(20);
    }
}
VERY IMPORTANT NOTE: Some of the addition and subtraction signs may need to be flipped to work for your robot, experiment with the signs when you write code for your robot.