Comp template full programs

```
 // ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// topfrontleft         inertial      8
// topbackright         motor         20
// topbackleft          motor         3
// frontleft            motor         17
// frontright           motor         9
// backright            motor         12
// backleft             motor         4
// intake               motor         2
// catapult             motor         1
// leftwing             digital_out   A
// intakepistonb        digital_out   B
// rightwing            digital_out   H
// Opticall             optical       5
// Controller1          controller
// RotationRight        rotation      13
// RotationLeft         rotation      14
// ---- END VEXCODE CONFIGURED DEVICES ----
/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       C:\Users\compl                                            */
/*    Created:      Mon Nov 27 2023                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// topfrontleft         inertial      8
// topbackright         motor         20
// topbackleft          motor         3
// frontleft            motor         17
// frontright           motor         9
// backright            motor         12
// backleft             motor         4
// intake               motor         2
// catapult             motor         1
// leftwing             digital_out   A
// intakepistonb        digital_out   B
// rightwing            digital_out   H
// Opticall             optical       10
// Controller1          controller
// RotationRight        rotation      13
// RotationLeft         rotation      14
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

// defining our functions

void lucadrive() {
catapult.setStopping(hold);
frontleft.setStopping(coast);
frontright.setStopping(coast);
backleft.setStopping(coast);
backright.setStopping(coast);
topbackleft.setStopping(coast);
topbackright.setStopping(coast);
while (true) {
float movement = (Controller1.Axis3.position()) + (Controller1.Axis1.position());
float movement1 = (Controller1.Axis3.position()) - (Controller1.Axis1.position());
frontleft.spin(forward, movement, pct);
frontright.spin(forward, movement1, pct);
//back dt
backleft.spin(forward, movement, pct);
backright.spin(forward, movement1, pct);
// ------------
topbackleft.spin(forward, movement, pct);
topbackright.spin(forward, movement1, pct);
// ------------
if (Controller1.ButtonR2.pressing()) {
(intake.spin(reverse, 90, pct));
} else if (Controller1.ButtonR1.pressing()) {
(intake.spin(forward, 90, pct));
} else {
intake.stop();
}
if (Controller1.ButtonY.pressing()) {
(catapult.spin(reverse, 90, pct));
} else if (Controller1.ButtonB.pressing()) {
(catapult.spin(forward, 90, pct));
} else {
catapult.stop();
}

//pnue intake
if (Controller1.ButtonLeft.pressing()) {
intakepistonb.set(true);
} else if (Controller1.ButtonDown.pressing()) {
intakepistonb.set(false);
}
//pnue wings
// all at once
if (Controller1.ButtonUp.pressing()) {
leftwing.set(true);
rightwing.set(true);
} else if (Controller1.ButtonLeft.pressing()) {
leftwing.set(false);
rightwing.set(false);
}
}
}

void forwardPIDD(float setpoint) {
float error = setpoint - backleft.position(degrees);
float power;
float i = 1;
double kP = 0.11;
while (abs(error) > i) {
error = setpoint - backleft.position(degrees);
power = error * kP;
Brain.Screen.printAt(60, 15, "%f", backleft.position(degrees));
topbackleft.spin(forward, power, pct);
topbackright.spin(forward, power, pct);
backleft.spin(forward, power, pct);
backright.spin(forward, power, pct);
frontleft.spin(forward, power, pct);
frontright.spin(forward, power, pct);
}
Brain.Screen.clearScreen();
Brain.Screen.printAt(60, 15, "%f", backleft.position(degrees));
frontleft.stop(coast);
frontright.stop(coast);
backleft.stop(coast);
backright.stop(coast);
topbackleft.stop(coast);
topbackright.stop(coast);
}

void autofire() {
while (true) {
if (Opticall.hue() < 105 && Opticall.hue() > 65) {
catapult.spin(forward, 50, pct);
Brain.Screen.print("Hue: %.2f", Opticall.hue());
Brain.Screen.setCursor(2,2);
}
else {
catapult.stop();
Brain.Screen.print("Hue: %.2f", Opticall.hue());
Brain.Screen.setCursor(2,2);
}

}
wait(10, msec);
Brain.Screen.clearScreen();
}

void sensordisplays () {
while (true) {
Brain.Screen.printAt(15, 15, "%f", RotationRight.position(degrees));
Brain.Screen.printAt(30, 30, "%f", RotationLeft.position(degrees));
Brain.Screen.printAt(45, 45, "%f", topfrontleft.heading(degrees));
Brain.Screen.printAt(60, 60, "Hue: %.2f", Opticall.hue());
wait(10,msec);
Brain.Screen.clearScreen();
}
}

void clearmotors () {
frontleft.resetPosition();
frontright.resetPosition();
backleft.resetPosition();
backright.resetPosition();
topbackleft.resetPosition();
topbackright.resetPosition();
}

void clearall() {
frontleft.resetPosition();
frontright.resetPosition();
backleft.resetPosition();
backright.resetPosition();
topbackleft.resetPosition();
topbackright.resetPosition();
topfrontleft.resetHeading();
RotationLeft.resetPosition();
RotationRight.resetPosition();
}

void setwings(int setpoint) {
leftwing.set(setpoint);
rightwing.set(setpoint);
}

void setintake(int setpoint) {
intakepistonb.set(setpoint);
}

void setall(int setpoint) {
leftwing.set(setpoint);
rightwing.set(setpoint);
intakepistonb.set(setpoint);
}

void pre() {
clearall();
// thread d(autofire);
Controller1.Screen.setCursor(2,2);
Controller1.Screen.print("Calibrating sensors...");
intakepistonb.set(false);
topfrontleft.calibrate();
wait(2.5, seconds);
Controller1.Screen.setCursor(2,2);
Controller1.Screen.print("      We're live!       ");
wait(3, seconds);
Controller1.Screen.setCursor(2,2);
Controller1.Screen.print("      Go Luca !!!       ");
}

void defensive() {
clearall();
setintake(true);
intake.spin(forward, 100, pct);
wait(.3, seconds);
intake.stop();
forwardPIDD(1850);
clearmotors();
wait(.1, seconds);
setintake(false);
turnPID(90, 15, -1);
clearmotors();
forwardPIDD(150);
clearmotors();
forwardPIDD(-600);
turnPID(0, 20, -1);
intake.spin(forward, 100, pct);
setintake(true);
clearmotors();
forwardPIDD(100);
}

void offensive() {
// begin route
setintake(true);
intake.spin(forward, 100, pct);
wait(.2, seconds);
intake.stop();
wait(.1, seconds);
// turn to net
turnPID(50, 15, -1);
clearall();
// score
forwardPIDD(850);
setintake(false);
forwardPIDD(1050);
clearmotors();
wait(.2,seconds);
//go back for bar
forwardPIDD(-600);
clearall();
turnPID(-45, 15, -1);
clearall();
//get back bar triball
setintake(false);
intake.spin(forward, 100, pct);
forwardPIDD(150);
clearall();
//turn and get *black* bar triball
turnPID(-93, 10, -1);
forwardPIDD(-1400);
clearall();
wait(0.1,seconds);
turnPID(92, 15, -1);
clearmotors();
wait(0.3, seconds);
forwardPIDD(150);
wait(0.2, seconds);
//collect it
intake.stop();
clearmotors();
turnPID(100, 15, -1);
forwardPIDD(-1400);
turnPID(0, 30, -1);
clearmotors();
setintake(true);
intake.spin(reverse, 90, pct);
forwardPIDD(350);
wait(.1, seconds);
clearmotors();
forwardPIDD(-900);
turnPID(0, 30, -1);
forwardPIDD(-900);
}

void skills() {
catapult.spin(reverse, 100, pct);
}

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*---------------------------------------------------------------------------*/

void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
pre();
thread a(sensordisplays);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

// This is the defensive
// defensive();

// This is the skills
// skills();

void autonomous(void) {
defensive();
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
// intakepistonb.set(true);
catapult.setStopping(hold);
frontleft.setStopping(coast);
frontright.setStopping(coast);
backleft.setStopping(coast);
backright.setStopping(coast);
topbackleft.setStopping(coast);
topbackright.setStopping(coast);
while (true) {
float movement = (Controller1.Axis3.position()) + (Controller1.Axis1.position());
float movement1 = (Controller1.Axis3.position()) - (Controller1.Axis1.position());
frontleft.spin(forward, movement, pct);
frontright.spin(forward, movement1, pct);
//back dt
backleft.spin(forward, movement, pct);
backright.spin(forward, movement1, pct);
// ------------
topbackleft.spin(forward, movement, pct);
topbackright.spin(forward, movement1, pct);
// ------------
if (Controller1.ButtonR2.pressing()) {
(intake.spin(reverse, 90, pct));
} else if (Controller1.ButtonR1.pressing()) {
(intake.spin(forward, 90, pct));
} else {
intake.stop();
}
if (Controller1.ButtonY.pressing()) {
(catapult.spin(reverse, 90, pct));
} else if (Controller1.ButtonB.pressing()) {
(catapult.spin(forward, 90, pct));
} else {
catapult.stop();
}

//pnue intake
if (Controller1.ButtonLeft.pressing()) {
intakepistonb.set(true);
} else if (Controller1.ButtonDown.pressing()) {
intakepistonb.set(false);
}
//pnue wings
// all at once
if (Controller1.ButtonUp.pressing()) {
leftwing.set(true);
rightwing.set(true);
} else if (Controller1.ButtonLeft.pressing()) {
leftwing.set(false);
rightwing.set(false);
}
}

while (1) {

wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}

// Main will set up the competition functions and callbacks.

int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);

// Run the pre-autonomous function.
pre_auton();

// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}
```