//Returns the lift back until shaft encoder count is 0
//Requires "ground" position to be 0 for this to work!
void returnLiftToPreload(){
while (abs(SensorValue[shaftEncoder]) > 45 && !killSwitch){
liftMotors(-1);
killSwitch = (vexRT[Btn8U] || vexRT[Btn8UXmtr2]) ? true : killSwitch;
}
stopLift();
}
void EnhancedShooter::doControls() {
if(*robotState != robot_class::NORMAL)
return; // NO conrols in climbing mode
//Lift Control
if(gunner -> GetRawButton(GUNNER_BTN_LIFT_UP))
{
setLiftPower(LIFT_POWER);
}
else if(gunner -> GetRawButton(GUNNER_BTN_LIFT_DOWN))
{
setLiftPower(-1.0f * LIFT_POWER);
}
else if(!liftTargetSet)
{
stopLift();
}
//Feeder Controls
if(gunner -> GetRawAxis(GUNNER_AXIS_FEEDER) > 0.98f) // right
{
setFeeder(FORWARD);
}
else if(gunner -> GetRawAxis(GUNNER_AXIS_FEEDER) < -0.98f) // left
{
setFeeder(BACKWARD);
}
}
//Raises or lowers the lift by a certain change in height,
//measured with the shaft encoder
void elevateLift(int height){
height *= -1;
int target = SensorValue[shaftEncoder] + height;
if (height < 0){
while (SensorValue[shaftEncoder] > target){
liftMotors(1);
}
stopLift();
} else {
while (SensorValue[shaftEncoder] < target){
liftMotors(-1);
}
stopLift();
}
stopLift();
}
void controlElevador(motor393 *motor_){ //funcion que controla los elevadores del robot
if(vexRT[ELEVAR]){
up(&motor_[motorLiftFront], &motor_[motorLiftBack]);
}
else if(vexRT[BAJAR]){
down(&motor_[motorLiftFront], &motor_[motorLiftBack]);
}
else stopLift(&motor_[motorLiftFront], &motor_[motorLiftBack]);
}
task main()
{
StartTask(intakeStart);
while(SensorValue[bumperLeft]==0)
{
}
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
wait10Msec(50);
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(200);
intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,600);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveFirstTierUp(127,1800);
moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 500);
if (time1[T4] < 10000)
{
moveSharpRight(127,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
}
StopTask(intakeStart);
}
/*
Raises or lowers the lift by a certain change in height,
measured with the shaft encoder
*/
void elevateLift(int deltaHeight){
int initialHeight = SensorValue[shaftEncoder];
if (deltaHeight > 0){
while (SensorValue[shaftEncoder] - initialHeight < deltaHeight){
liftMotors(1);
}
} else {
while (SensorValue[shaftEncoder] - initialHeight > deltaHeight){
liftMotors(-1);
}
}
stopLift();
}
bool controlShot(motor393 *motor_, bool dis){ //funcion que controla los disparadores
if(vexRT[DISPARO] && dis){
startTask(shot); //llamo la tarea que controla los disparadores para que funcione de forma paralela a main
dis=false;
waitUntil(!vexRT[DISPARO]);//espera hasta que suelte el boton
}
else if(vexRT[DISPARO]){
stopTask(shot);//detengo el oscilamiento
stopLift(&motor_[motorShotRight], &motor_[motorShotLeft]); //detengo los motores
dis=true;
waitUntil(!vexRT[DISPARO]);//espera hasta que suelte el boton
}
return dis;
}
task autonomous()
{
motor393 motor_[10];//declaro los motores
short fronts[10]={1, FRONTLEFT, FRONTRIGHT, FRONTOMNI, FRONTSUC, FRONTLIFTFRONT, FRONTLIFTBACK, FRONTSHOTRIGHT, FRONTSHOTLEFT, 1};
short ports[10]={0,motorLeft,motorRight,motorOmni,motorSuc,motorLiftFront,motorLiftBack,motorShotRight,motorShotLeft,0};
initialize(&motor_[0], &fronts[0], &ports[0]);
startSpeed(&motor_[0]);
up(&motor_[motorShotRight], &motor_[motorShotLeft]);
front(motor_);
delay(8000);
stopRobot(motor_);
motorFront(&motor_[motorSuc]);
delay(2000);
stopLift(&motor_[motorShotRight], &motor_[motorShotLeft]);
back(motor_);
delay(500);
turnback(motor_);
delay(2600);
while(true){
stopLift(&motor_[motorShotRight], &motor_[motorShotLeft]);
front(motor_);
}
}
void EnhancedShooter::update() {
if(*robotState == robot_class::NORMAL)
{
if(liftTargetSet)
{
if(atPot(liftTarget))
{
liftTargetSet = false;
stopLift();
}
else if(lift.GetPosition() < liftTarget)
{
lift.Set(0.3141592654);
}
else
{
lift.Set(-1.0f * 0.3141592654);
}
}
if(HalEffect.Get() > 0)
{
stopFeeder();
HalEffect.Reset();
HalEffect.Start();
}
if(gunner -> GetRawButton(GUNNER_BTN_SHOOTER_WHEEL_REV)) {
wheelForward = false;
setSpeed(-1.0f * WHEEL_POWER);
}
else if(wheelForward)
{
setSpeed(WHEEL_POWER);
}
else
{
stopWheel();
}
}
else
{
setLiftPower(-1.0f * LIFT_POWER); //Limit Switch Will Stop It
stopFeeder();
stopWheel();
}
}
void legs(){
if (bot == lift){
if (joystickGetDigital(2, 7, JOY_UP)){
drinv = 1;
}
if (joystickGetDigital(2, 7, JOY_DOWN)){
drinv = -1;
}
if (joystickGetDigital(2, 7, JOY_LEFT)){
oneleg_hold = 1;
otherleg_hold = 1;
}
if (joystickGetDigital(2, 7, JOY_RIGHT)){
oneleg_hold = 0;
otherleg_hold = 0;
}
int lflgo = joystickGetDigital(2, 5, JOY_UP);
int lrlgo = joystickGetDigital(2, 5, JOY_DOWN);
int rflgo = joystickGetDigital(2, 6, JOY_UP);
int rrlgo = joystickGetDigital(2, 6, JOY_DOWN);
if (oneleg_hold){
if (lflgo){
motorSet(left_front_leg, drinv * left_front_leg_speed);
} else if (joystickGetDigital(2, 8, JOY_UP)){
oneleg_hold = 0;
} else {
motorSet(left_front_leg, lflhold);
}
} else {
if (joystickGetDigital(2, 8, JOY_UP)){
motorSet(left_front_leg, -left_front_leg_speed);
} else if (lflgo){
motorSet(left_front_leg, drinv * left_front_leg_speed);
} else {
motorSet(left_front_leg, 0);
}
}
if (lrlgo){
motorSet(left_rear_leg, drinv * left_rear_leg_speed);
} else {
if (otherleg_hold){
motorSet(left_rear_leg, lrlhold);
} else {
motorSet(left_rear_leg, 0);
}
}
if (rflgo){
motorSet(right_front_leg, drinv * right_front_leg_speed);
} else {
if (otherleg_hold){
motorSet(right_front_leg, rflhold);
} else {
motorSet(right_front_leg, 0);
}
}
if (rrlgo){
motorSet(right_rear_leg, drinv * right_rear_leg_speed);
} else if (joystickGetDigital(2, 8, JOY_RIGHT)){
motorSet(right_rear_leg, -right_rear_leg_speed);
} else {
if (otherleg_hold){
motorSet(right_rear_leg, rrlhold);
} else {
motorSet(right_rear_leg, 0);
}
}
} else {
stopLift();
}
}
task autonomous(){
scoredSections = 0;
direction = 1;
SensorValue[pneuVal] = 0;
if (SensorValue[disableAut] == 1){
//If we are on the blue side, preload and scoring are switched
//To fix this, switch the direction of all horizontal motor movements
if (SensorValue[nextToSection] == 0){ //If its plugged in
//Switch direction so preload is now on the other side and we can use the same code
direction = -1;
}
bool firstTime = true;
while(scoredSections < 6){
if (firstTime){
//Release the arm only on the first time
if (direction == -1){
motor[arm] = armSpeedAutonomous;
wait1Msec(200);
motor[arm] = 0;
}
//Raise the lift slightly (for 0.8 sec) to expand arm
liftMotors(1);
wait1Msec(600);
//Open the clamp
SensorValue[pneuVal] = 1;
stopLift();
motor[arm] = direction * -armSpeedAutonomous;
wait1Msec(250);
motor[arm] = 0;
}
//Move arm to pick up section
pickUpSection();
//Start lifting process
//This needs to be our '0' spot so we come back to here
SensorValue[shaftEncoder] = 0;
//Regression Quadratic Equation
int calculatedHeight;
if (firstTime){
calculatedHeight = 235;
} else{
calculatedHeight = (289.0 * scoredSections) + 85.33;
}
elevateLift(calculatedHeight);
//Scoring
moveArmToScore(direction * -1 * armSpeedUserControl);
//if (scoredSections >= 2){
// wait1Msec(100);
//}
int nestValue = (firstTime) ? -330 : (int) (-867/7);
elevateLift(nestValue);
wait1Msec(50);
SensorValue[pneuVal] = 1;
int clearValue = (firstTime) ? 200 : 230;
firstTime = false;
elevateLift(clearValue);
//Return to starting position
//get the arm going in that direction for a bit so it doesn't get caught on anything
//motor[arm] = (direction * armSpeedAutonomous);
//wait1Msec(400);
//motor[arm] = 0;
clearSkyrise();
returnLiftToPreload();
scoredSections++;
}
}
}
//Returns the lift back until shaft encoder count is 0
//Requires "ground" position to be 0 for this to work!
void returnLiftToPreload(){
while (abs(SensorValue[shaftEncoder]) > 45){
liftMotors(-1);
}
stopLift();
}
//Uses the button sensor
void returnLiftToGround(){
while (SensorValue[shaftEncoder] > 0){ //While its not pressed
liftMotors(-1);
}
stopLift();
}
void EnhancedShooter::stopAll() {
stopWheel();
stopFeeder();
stopLift();
}
task autonomous()
{
// .....................................................................................
// Insert user code here.
// .....................................................................................
if(programs ==0)
{
StartTask(intakeStart);
/*while(SensorValue[bumperLeft]==0)
{
}
*/
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
motor[secondTier]=-127;
wait10Msec(50);
motor[secondTier]=0;
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(150);
//intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,-10,650);
moveStraightDistance(127,100);
turnRight(127,100);
stopPid(0.6,0.3);
wait10Msec(50);
//moveFirstTierUp(127,1800);
//moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 1000);
moveSharpRight(127,0,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
StopTask(intakeStart);
}
else if(programs ==1)
{
}
else if(programs ==2)
{
}
}
