Skip to content

Instantly share code, notes, and snippets.

@catb0t
Created November 2, 2016 20:45
Show Gist options
  • Save catb0t/29821ddd03e6157c573b945039b45845 to your computer and use it in GitHub Desktop.
Save catb0t/29821ddd03e6157c573b945039b45845 to your computer and use it in GitHub Desktop.
#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