void pre_auton( void ) {
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
// RUN THE LIFT TO THE BOTTOM LIMIT SWITCH AND THEN SET THE REVOLUTIONS TO ZERO.
// initLift(); THIS CODE IS CAUSING MAJOR PROBLEMS – REMOVE IT FOR NOW – ON STARTUP MAKE SURE LIFT IS FULLY DOWN!
Brain.Screen.print(“PRE-AUTON: %4.2f”,Lift.rotation(rotationUnits::rev));
Brain.Screen.newLine();
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
}
void autonomous( void ) {
// …
// Insert autonomous user code here.
// …
//blue small is port A
//blue large is port B
//red small is port C
//red large is port D
/=================================================================================================/
// BLUE ALLIANCE (SMALL GOAL)
// Starting position: start on right edge of first square on the left with back touching wall
// Pre-load: cube in closed claw
/=================================================================================================/
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
}
void usercontrol( void ) {
// User control code here, inside the loop
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
while (1){
// normal user control stuff in here
}
When you run this:
BS: 0 BL: 0 RS: 0 RL: 0 (from pre-auton)
BS: 1 BL: 0 RS: 0 RL: 0 (from auton)
BS: 1 BL: 0 RS: 0 RL: 0 (from user control)
NOTE: that there is a jumper in port A, which should always return 1.
If I remove the print statement from pre-auton, then the result is this
BS: 0 BL: 0 RS: 0 RL: 0 (from auton)
BS: 1 BL: 0 RS: 0 RL: 0 (from user control)
The above are snippets. The full code is below:
#define _USE_MATH_DEFINES
#include “robot-config.h”
/---------------------------------------------------------------------------/
/* /
/ Description: Competition template for VCS VEX V5 /
/ /
/---------------------------------------------------------------------------*/
/---------------------------------------------------------------------------/
/* 2019 Tower Takeover /
/---------------------------------------------------------------------------/
/ /
/ Summer 2019 Configuration /
/ 6 wheel robot – driven on center friction wheels /
/ port 1 – Left Drive /
/ port 10 – Right Drive (reversed) /
/ Claw – single side rotation for open and close /
/ port 8 /
/ Lift – Linear Lift – provides 10-1/8 inches of lift /
/---------------------------------------------------------------------------/
/ Change Log /
/---------------------------------------------------------------------------/
/ /
/ 7-24-2019 Dylan, Andy, Jonas /
/ Initial Version /
/ User Control of Drive Train, Lift, and Claw /
/ Beginnings of Autonomous Mode /
/ /
/ 7-28-2019 Andy Borden /
/ Added code for autonomous driving /
/ driveStraight function completed /
/ turnRobot function completed /
/ /
/ 7-29-2019 Andy Borden and Dylan Waldman /
/ Added the lift code for autonomous /
/ ARE REV THE RIGHT UNITS /
/ /
/ 7-30-2019 COACHES NOTES /
/ NOTES WITH THIS HEADING HAVE BEEN ADDED TO THE CODE /
/ STUDENTS SHOULD REFER TO NOTES AND MAKE NECESSARY CHANGES /
/ /
/---------------------------------------------------------------------------*/
//Creates a competition object that allows access to Competition methods.
vex::competition Competition;
/=================================================================================================/
// CONSTANTS
/=================================================================================================/
const double K_WHEEL_DIAMETER = 4.0;
const double K_TURNING_TWEAK = 1.0;
const double K_HALF_WHEEL_BASE = 7.3125; // 13.75" - 15.5" Use 14.625
const double MAX_LIFT_INCHES = 10.125;
const double MAX_LIFT_REV_VALUE = -6.450;
double currentPower = 1;
/=================================================================================================/
// initLift
// moves the lift all the way down until it hits the limit switch
// stop the motor
// set the revolutions to 0
// Written by Andy on 7-30-2019
/=================================================================================================/
void initLift() {
Brain.Screen.print(DownLimit.pressing());
Brain.Screen.newLine();
if (!DownLimit.pressing()) Lift.spin(directionType::fwd, 40, velocityUnits::pct); // DOWN
while (!DownLimit.pressing())
{
Brain.Screen.print(DownLimit.pressing());
Brain.Screen.newLine();
}
Lift.stop(brakeType::brake);
Lift.resetRotation();
}
/---------------------------------------------------------------------------/
/* 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 cortex has been powered on and /
/ not every time that the robot is disabled. /
/---------------------------------------------------------------------------*/
void pre_auton( void ) {
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
// RUN THE LIFT TO THE BOTTOM LIMIT SWITCH AND THEN SET THE REVOLUTIONS TO ZERO.
// initLift(); THIS CODE IS CAUSING MAJOR PROBLEMS – REMOVE IT FOR NOW – ON STARTUP MAKE SURE LIFT IS FULLY DOWN!
Brain.Screen.print(“PRE-AUTON: %4.2f”,Lift.rotation(rotationUnits::rev));
Brain.Screen.newLine();
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
}
/=================================================================================================/
// openClaw
// Move the claw to the open position and hold it open
// NOTE: only the right side of the claw opens
/=================================================================================================/
void openClaw() {
Claw.setStopping(brakeType::hold);
Claw.setVelocity(20, velocityUnits::pct);
Claw.rotateTo(-0.098,rotationUnits::rev);
}
/=================================================================================================/
// closeClaw
// Move the claw to the closed position and hold it open
// NOTE: only the right side of the claw closes
/=================================================================================================/
void closeClaw() {
Claw.setStopping(brakeType::hold);
Claw.setVelocity(20, velocityUnits::pct);
Claw.rotateTo(0.075,rotationUnits::rev);
}
/=================================================================================================/
// driveStraight
// NOTE: This function is based on theoretical distance based on tire diameter
// 7-29 - swapped the parameters Speed and Distance so that Distance comes first
/=================================================================================================/
void driveStraight(double Distance, double Speed, bool Blocking){
//velocity in inches per second
//distance in inches, negative numbers will make you drive backwards
double circumference = K_WHEEL_DIAMETER * M_PI;
double degreesPerInch = 360 / circumference;
RWheels.setVelocity(Speed * degreesPerInch, velocityUnits::dps);
LWheels.setVelocity(Speed * degreesPerInch, velocityUnits::dps);
RWheels.rotateFor(Distance * degreesPerInch, rotationUnits::deg, false);
LWheels.rotateFor(Distance * degreesPerInch, rotationUnits::deg, Blocking);
//the boolean "blocking", if true, will wait to finish executing the function before moving on to the next line of code.
}
/=================================================================================================/
// turnRobot
// NOTE: This function is based on wheel diameter and wheel distance from center
// K_TURNING_TWEAK is an adjustment factor.
// 7/29 - Changed Arm to constant K_ARM, added speed as a parameter to the function
// 7-30 - Changed K_ARM to K_HALF_WHEEL_BASE
/=================================================================================================/
void turnRobot(double Degrees, double Speed, bool Blocking){
//negative numbers will make you turn to the left
//A=robot rotation (in degrees)
//D=wheel diameter
//W=1/2 wheel base
//T= wheel rotation (in degrees)
//M= linear distance of wheel movement
//robot rotation: A=(M/W)*(180/pi)
//wheel movement: M=T*(pi/180)*D/2
//substituting: A=((T*(pi/180)*D/2)/W)*(180/pi)
//cancelling: A=(T*D/2)/W
//simplifying: A=TD/2W
//rearranging: T=2WA/D
double turnAmount = Degrees * K_TURNING_TWEAK * 2 * K_HALF_WHEEL_BASE / K_WHEEL_DIAMETER;
double turnSpeed = Speed * K_TURNING_TWEAK * 2 * K_HALF_WHEEL_BASE / K_WHEEL_DIAMETER;
RWheels.setVelocity(turnSpeed, velocityUnits::dps);
LWheels.setVelocity(turnSpeed, velocityUnits::dps);
RWheels.rotateFor(-turnAmount, rotationUnits::deg, false);
LWheels.rotateFor(turnAmount, rotationUnits::deg, Blocking);
}
/=================================================================================================/
// moveLift
// moves the lift to an absolute height in inches
// Written by Andy on 7-30-2019
// 0 revolutions = 0 inches (lift at the bottom)
// -6.450 revolutions = 10.125 inches (lift at the top)
// every > and < needs to be checked, we might want to reverse this motor
/=================================================================================================/
void moveLift(double Inches) {
// NOTE: VELOCITY IS NEGATIVE WHEN LIFT IS RISING
double height = MAX_LIFT_REV_VALUE / MAX_LIFT_INCHES * Inches;
if (height <= 0.00 && height >= MAX_LIFT_REV_VALUE) Lift.rotateTo(height, rotationUnits::rev, 40, velocityUnits::pct, true);
else Brain.Screen.print("LIFT OUT OF BOUNDS");
Brain.Screen.newLine();
Brain.Screen.print("height: %4.2f", height);
Brain.Screen.newLine();
// WE SHOULD BE CHECKING THE LIMIT SWITCHES -- HOWEVER, THIS CODE HAS BEEN PROBLEMATIC
// WE WILL ADDRESS AT A LATER DATE!
/*===========================================================
Lift.startRotateTo(height, rotationUnits::rev, 40, velocityUnits::pct);
while (Lift.isSpinning())
{
if (Lift.velocity(velocityUnits::pct) > 0) // GOING DOWN -- CHECK DOWNLIMIT
{
if (DownLimit.pressing() || Lift.rotation(rotationUnits::rev) >= height) Lift.stop();
}
if (Lift.velocity(velocityUnits::pct) < 0) // GOING UP -- CHECK UPLIMIT
{
if (UpLimit.pressing() || Lift.rotation(rotationUnits::rev) <= height) Lift.stop();
}
}
=============================================================*/
}
double stickAdjust(double StickIn) {
return StickIn * 100/127 * currentPower;
}
/---------------------------------------------------------------------------/
/* /
/ 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. /
/---------------------------------------------------------------------------*/
void autonomous( void ) {
// …
// Insert autonomous user code here.
// …
//blue small is port A
//blue large is port B
//red small is port C
//red large is port D
/=================================================================================================/
// BLUE ALLIANCE (SMALL GOAL)
// Starting position: start on right edge of first square on the left with back touching wall
// Pre-load: cube in closed claw
/=================================================================================================/
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
/*
Brain.Screen.print(AutonBlueSmall.pressing());
Brain.Screen.newLine();
Brain.Screen.print(AutonBlueLarge.pressing());
Brain.Screen.newLine();
Brain.Screen.print(AutonRedSmall.pressing());
Brain.Screen.newLine();
Brain.Screen.print(AutonRedLarge.pressing());
*/
//******************Auton Blue Small *********************Slot 1
if (true) {
// Close claw
closeClaw();
// Move robot forwards 24 inches
driveStraight(24, 10, true);
// Turn robot counterclockwise 135 degrees
turnRobot(-135, 60, true);
// Move robot forward 25 inches
driveStraight(25, 10, true);
// Open the claw and let the cube go
openClaw();
// Back up 6 inches
driveStraight(-6, 10, true);
// Close the claw
closeClaw();
// Move forward 14 inches and push the block
driveStraight(14, 10, true);
// Turn our robot clockwise 15 degrees
turnRobot(15, 60, true);
// Move robot backwards
driveStraight(-6, 10, true);
}
/=================================================================================================/
// BLUE ALLIANCE (LARGE GOAL)
// Starting position: start on the left side of the first tile to the right, with the right of the robot touching the wall
// Pre-load: cube in closed claw
/=================================================================================================/
if (false) {
driveStraight (14, 10, true);
driveStraight (-14, 10, true);
}
/=================================================================================================/
// RED ALLIANCE (SMALL GOAL)
// Starting position: start on left edge of first square on the right with back touching wall
// Pre-load: cube in closed claw
/=================================================================================================/
if (false) {
// Close claw
closeClaw();
// Move robot forwards 24 inches
driveStraight(24, 10, true);
// Turn robot clockwise 135 degrees
turnRobot(135, 60, true);
// Move robot forward 25 inches
driveStraight(25, 10, true);
//open the claw
openClaw();
//move backwards 6 inches
driveStraight(-6, 10, true);
//close the claw
closeClaw();
//move forwards 14 inches
driveStraight(14, 10, true);
//turn the robot counterclockwise 15 degrees
turnRobot(-15,60,true);
// Move the robot backwards 6 inches
driveStraight(-6, 10, true);
}
/=================================================================================================/
// RED ALLIANCE (LARGE GOAL)
// Starting position: start on the left edge of the first tile on the right with the left side of the robot touching the wall
// Pre-load: cube on closed claw
/=================================================================================================/
if (false)
{
driveStraight (14, 10, true);
driveStraight (-14, 10, true);
}
}
/----------------------------------------------------------------------------/
/* /
/ 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 ) {
// User control code here, inside the loop
Brain.Screen.print(“BS: %d BL: %d RS: %d RL: %d”, AutonBlueSmall.pressing(), AutonBlueLarge.pressing(), AutonRedSmall.pressing(), AutonRedLarge.pressing());
Brain.Screen.newLine();
while (1){
// 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.
// ........................................................................
/*=================================================================================================*/
// DRIVE TRAIN (2 STICK ARCADE)
// AXIS3 (LEFT STICK Y-AXIS): SPEED
// AXIS1 (RIGHT STICK X-AXIS): AMOUNT OF TURN
/*=================================================================================================*/
/*---------------------------------------------------------------------------*/
/* 7-30-2019 COACHES NOTES */
/* 1) THIS IS REALLY SUPER FAST THINK ABOUT ADDING A PRECISION */
/* BUTTON THAT WILL SLOW IT WAY WAY DOWN */
/* 2) THE CONTROLLER RETURNS AN AXIS VALUE OF -127 TO 127 */
/* THIS IS NOT A PERCENTAGE -- REACH FULL SPEED LONG BEFORE */
/* THE STICK IS PUSHED TO ITS EXTREME */
/*---------------------------------------------------------------------------*/
if (Controller1.ButtonA.pressing()) currentPower = 1;
if (Controller1.ButtonB.pressing()) currentPower = 0.5;
LWheels.spin(directionType::fwd, stickAdjust(Controller1.Axis3.value() + Controller1.Axis1.value()), velocityUnits::pct); //LEFT SIDE
RWheels.spin(directionType::fwd, stickAdjust(Controller1.Axis3.value() - Controller1.Axis1.value()), velocityUnits::pct); //RIGHT SIDE
/*=================================================================================================*/
// LIFT
// L1: UP LIMITED BY LIMIT SWITCH UpLimit
// L2: DOWN LIMITED BY LIMIT SWITCH DownLimit
/*=================================================================================================*/
if (Controller1.ButtonL1.pressing()) //UP
{
if (UpLimit.pressing()) Lift.stop();
else Lift.spin(directionType::rev,40,velocityUnits::pct);
}
else if (Controller1.ButtonL2.pressing()) //DOWN
{
if (DownLimit.pressing()) Lift.stop();
else Lift.spin(directionType::fwd,40,velocityUnits::pct);
}
else Lift.stop();
/*=================================================================================================*/
// CLAW
// R1: OPEN
// R2: CLOSE
/*=================================================================================================*/
if (Controller1.ButtonR1.pressing()) openClaw() ; // OPEN
else if (Controller1.ButtonR2.pressing()) closeClaw(); // CLOSE
else Claw.stop(); // STOP
vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
//Run the pre-autonomous function.
pre_auton();
//Set up callbacks for autonomous and driver control periods.
Competition.autonomous( autonomous );
Competition.drivercontrol( usercontrol );
//Prevent main from exiting with an infinite loop.
while(1) {
vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
}
}
