Created
November 5, 2016 12:39
-
-
Save catb0t/acfa6d184870edf0bc7f89b9173d2266 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, , tmotorVex393_HBridge, openLoop) | |
#pragma config(Motor, port2, m_clawL, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port3, , tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port4, m_armL, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port5, m_drvR, tmotorVex393_MC29, openLoop, reversed) | |
#pragma config(Motor, port6, m_drvL, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port7, m_armR, tmotorVex393_MC29, openLoop, reversed) | |
#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port9, m_clawR, tmotorVex393_MC29, openLoop, reversed) | |
#pragma config(Motor, port10, , tmotorVex393_HBridge, openLoop) | |
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// | |
// 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" | |
#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)) | |
void pre_auton() | |
{ | |
bStopTasksBetweenModes = true; | |
} | |
task autonomous() { | |
set2Mtrs(m_claw, 127); | |
wait1Msec(400); | |
set2Mtrs(m_claw, 0); | |
wait1Msec(10); | |
set2Mtrs(m_arm, -127); | |
wait1Msec(500); | |
set2Mtrs(m_arm, 0); | |
wait1Msec(10); | |
setMtr(m_drvL, 127); | |
setMtr(m_drvR, -127); | |
wait1Msec(700); | |
set2Mtrs(m_drv, 0); | |
} | |
// Holds claw up | |
void flatClaw(bool armHold) { | |
if ( BTN_START_ARMHOLD ) { | |
armHold = true; | |
} else if ( BTN_STOP_ARMHOLD ) { | |
armHold = false; | |
} | |
if (armHold) { | |
set2Mtrs(m_claw, -31); | |
} | |
} | |
// moves claw | |
void clawMotion(void) { | |
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); | |
} | |
} | |
#define RIGHT_HORIZ vexRT[Ch1] | |
#define RIGHT_VERT vexRT[Ch2] | |
#define LEFT_VERT vexRT[Ch3] | |
#define LEFT_HORIZ vexRT[Ch4] | |
task usercontrol () { | |
bool armHold = false; | |
short speed = 10; | |
while ( true ) { | |
int rJoy_rhs = +(RIGHT_VERT - RIGHT_HORIZ); | |
int rJoy_lhs = -(RIGHT_VERT + RIGHT_HORIZ); | |
int lJoy_bhs = LEFT_VERT; | |
flatClaw(armHold); | |
clawMotion(); | |
setMtr(m_drvL, (int) rJoy_rhs / 1.5); | |
setMtr(m_drvR, (int) rJoy_lhs / 1.5); | |
setMtr(m_armL, (int) lJoy_bhs / 1.5); | |
setMtr(m_armR, (int) lJoy_bhs / 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