Created
November 2, 2016 20:45
-
-
Save catb0t/29821ddd03e6157c573b945039b45845 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#pragma config(Motor, port1, m_clawL, tmotorVex393_HBridge, openLoop) | |
#pragma config(Motor, port2, m_drvL, tmotorVex393_MC29, openLoop, reversed) | |
#pragma config(Motor, port3, , tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port4, , tmotorVex393_MC29, openLoop, reversed) | |
#pragma config(Motor, port5, m_armL, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port6, m_armR, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port7, , tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port9, m_drvR, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port10, m_clawR, tmotorVex393_HBridge, openLoop) | |
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// | |
/*---------------------------------------------------------------------------*/ | |
/* */ | |
/* Description: Competition template for VEX EDR */ | |
/* */ | |
/*-------------------------------------------------------- | |
------------------*/ | |
// This code is for the VEX cortex platform | |
#pragma platform(VEX2) | |
// Select Download method as "competition" | |
#pragma competitionControl(Competition) | |
//Main competition background code...do not modify! | |
#include "Vex_Competition_Includes.c" | |
/*---------------------------------------------------------------------------*/ | |
/* 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() | |
{ | |
// Set bStopTasksBetweenModes to false if you want to keep user created tasks | |
// running between Autonomous and Driver controlled modes. You will need to | |
// manage all user created tasks if set to false. | |
bStopTasksBetweenModes = true; | |
// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD | |
// used by the competition include file, for example, you might want | |
// to display your team name on the LCD in this function. | |
// bDisplayCompetitionStatusOnLcd = false; | |
// 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. */ | |
/*---------------------------------------------------------------------------*/ | |
task autonomous() | |
{ | |
// .......................................................................... | |
// Insert user code here. | |
// .......................................................................... | |
// Remove this function call once you have "real" code. | |
AutonomousCodePlaceholderForTesting(); | |
} | |
/*---------------------------------------------------------------------------*/ | |
/* */ | |
/* 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. */ | |
/*---------------------------------------------------------------------------*/ | |
#define BTN_START_ARMHOLD vexRT[Btn7R] | |
#define BTN_STOP_ARMHOLD vexRT[Btn7D] | |
#define BTN_CLW_SLUP vexRT[Btn6D] | |
#define BTN_CLW_FLUP vexRT[Btn6U] | |
#define BTN_CLW_SLDN vexRT[Btn5D] | |
#define BTN_CLW_FLDN vexRT[Btn5U] | |
#define setMtr(name, value) motor[(name)] = (value) | |
#define set2Mtrs(name, value) \ | |
setMtr(name ## L, (value)); \ | |
setMtr(name ## R, (value)) | |
#define iabs(x) ((x) < 0 ? -(x) : x) | |
// Holds claw up | |
bool flatClaw(bool armHold) { | |
if ( BTN_START_ARMHOLD ) { | |
armHold = true; | |
} else if ( BTN_STOP_ARMHOLD ) { | |
armHold = false; | |
} | |
if (armHold) { | |
set2Mtrs(m_claw, -31); | |
} | |
return armHold; | |
} | |
// moves claw | |
short clawMotion(short speed) { | |
if ( BTN_CLW_FLUP ) { // regular move up | |
set2Mtrs(m_claw, 127); | |
} else if ( BTN_CLW_SLUP ) { | |
set2Mtrs(m_claw, 63); | |
} else if ( BTN_CLW_FLDN ) { // regular move down | |
set2Mtrs(m_claw, -127); | |
} else if ( BTN_CLW_SLDN ) { //slow move claw down | |
set2Mtrs(m_claw, -63); | |
} else { | |
set2Mtrs(m_claw, 0); | |
} | |
return speed; | |
} | |
#define RIGHT_VERT vexRT[Ch2] | |
#define RIGHT_HORIZ vexRT[Ch1] | |
#define LEFT_HORIZ vexRT[Ch3] | |
task usercontrol () { | |
int joyStickDeadZone = 10; // setup the joystick Dead Zone | |
nMotorPIDSpeedCtrl[m_drvL] = mtrNoReg; // turn off PID for motors | |
nMotorPIDSpeedCtrl[m_drvR] = mtrNoReg; | |
nMotorPIDSpeedCtrl[m_armFL] = mtrNoReg; | |
nMotorPIDSpeedCtrl[m_armFR] = mtrNoReg; | |
slaveMotor(m_armFR, m_armBR); | |
slaveMotor(m_armFL, m_armBL); | |
int leftJoy = 0, rightJoy = 0, rArmCtrl = 0, lArmCtrl = 0; | |
bool armHold = false; | |
short speed = 0; | |
while ( true ) { | |
leftJoy = -(RIGHT_VERT + RIGHT_HORIZ); | |
rightJoy = -(RIGHT_VERT - RIGHT_HORIZ); | |
rArmCtrl = -(LEFT_HORIZ); | |
lArmCtrl = -(LEFT_HORIZ); | |
armHold = flatClaw(armHold); | |
speed = clawMotion(speed); | |
// check to see if left value is within dead zone. | |
// if in the dead zone then zero it out | |
if ( iabs(leftJoy) <= joyStickDeadZone) { | |
leftJoy = 0; | |
} | |
if ( iabs(rightJoy) <= joyStickDeadZone) { | |
rightJoy = 0; | |
} | |
if ( iabs(rArmCtrl) <= joyStickDeadZone) { | |
rArmCtrl = 0; | |
} | |
if ( iabs(lArmCtrl) <= joyStickDeadZone) { | |
lArmCtrl = 0; | |
} | |
setMtr(m_drvL, (int) rightJoy / 1.5); | |
setMtr(m_drvR, (int) leftJoy / 1.5); | |
setMtr(m_armLF | |
set2Mtrs(m_armB, (int) rArmCtrl / 1.5); | |
set2Mtrs(m_armF, (int) lArmCtrl / 1.5); | |
wait1Msec(10); // allow task to sleep (don't starve the other tasks). | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment