#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
motor leftB = motor(PORT11, ratio18_1, true);
motor leftM = motor(PORT1, ratio18_1, true);
motor leftF = motor(PORT2, ratio18_1, true);
motor_group DriveL = motor_group(leftB, leftM, leftF);
motor rightB = motor(PORT20, ratio18_1, false);
motor rightM = motor(PORT10,ratio18_1, false);
motor rightF = motor(PORT9, ratio18_1, false);
motor_group DriveR = motor_group(rightB, rightM, rightF);
motor Intake = motor(PORT7, ratio6_1, false);
controller Controller1 = controller(primary);
inertial Inertial = inertial(PORT4);
motor Flywheel = motor(PORT6, ratio6_1,false);
pneumatics Clamp = pneumatics(Brain.ThreeWirePort.A);
extern brain Brain;
// VEXcode devices
extern controller Controller1;
extern drivetrain Drivetrain;
//extern motor Intake;
extern inertial Inertial;
extern motor_group DriveL;
extern motor_group DriveR;
//extern motor Flywheel;
extern motor rightB;
extern motor rightF;
extern motor rightM;
extern motor leftB;
extern motor leftF;
extern motor leftM;
extern motor Intake;
/*extern pneumatics WingR;
extern pneumatics WingL;/*
/**
* Used to initialize code/tasks/devices added using tools in VEXcode Pro.
*
* This should be called at the start of your int main function.
*/
void vexcodeInit( void );
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
//TURN PID
void turnPID(double angle){
double current = 0;
double error = (angle - current);
double kp = .18;
double speed = 0;
Inertial.resetHeading();
while(fabs(error)>= 3){
if(fabs(error) >= 180){
error = -fabs(error)/error * 360 - fabs(error);
}
current = Inertial.heading(deg);
speed = kp*error;
rightB.spin(reverse,speed, volt);
rightF.spin(reverse,speed, volt);
rightM.spin(reverse,speed, volt);
leftB.spin(fwd,speed, volt);
leftF.spin(fwd,speed, volt);
leftM.spin(fwd,speed, volt);
error = (angle - current);
task::sleep(15);
}
rightB.stop(brake);
rightM.stop(brake);
rightF.stop(brake);
leftB.stop(brake);
leftM.stop(brake);
leftF.stop(brake);
}
/**/
//DRIVE PID
void drivePID(double target){
double current = 0;
double error = target-current;
double kp = .04; //change this//
double speed = 0;
rightB.resetPosition();
while(fabs(error)> 3){
current = rightB.position(deg);
speed = kp*error;
rightB.spin(fwd,speed, volt);
rightF.spin(fwd,speed, volt);
rightM.spin(fwd,speed, volt);
leftB.spin(fwd,speed, volt);
leftF.spin(fwd,speed, volt);
leftM.spin(fwd,speed, volt);
error = (target - current);
task::sleep(15);
}
rightB.stop(brake);
rightM.stop(brake);
rightF.stop(brake);
leftB.stop(brake);
leftM.stop(brake);
leftF.stop(brake);
}
/*AUTON*/
void autonomous(void) {
drivePID(4000);
wait(.3,sec);
turnPID(90);
wait(.3,sec);
drivePID(-360);
wait(.3, sec);
Intake.spin(reverse);
wait(2,sec);
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
float throttle = 0;
float turn = 0;
void usercontrol(void) {
Drivetrain.setDriveVelocity(60,pct);
Drivetrain.setStopping(brake);
Drivetrain.setTurnVelocity(60,pct);
// User control code here, inside the loop
while (1) {
throttle = Controller1.Axis3.value();
turn = Controller1.Axis1.value();
DriveL.spin(fwd, throttle + turn, pct);
DriveR.spin(fwd, throttle - turn, pct);
if(Controller1.ButtonDown.pressing()){
Clamp.open();
}
else if(Controller1.ButtonDown.pressing(false)){
Clamp.close();
}
if(Controller1.ButtonL2.pressing()) {
Intake.spin(fwd,6,volt);
}
else if(Controller1.ButtonL1.pressing()) {
Intake.spin(reverse,6,volt);
}
else {
Intake.stop();
}
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
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);
}
}
There shouldn’t be a semicolon there, since that makes it compile as two separate lines. Also, you never end the usercontrol function, so your main function is inside it, which could cause issues. What is the exact error you are getting?
1 Like