Skip to content

Instantly share code, notes, and snippets.

@jononon

jononon/2017MarkIII.c

Last active February 9, 2017 21:23
Show Gist options
  • Save jononon/e1cec2797bfa106f5c2369c6e2072283 to your computer and use it in GitHub Desktop.
Save jononon/e1cec2797bfa106f5c2369c6e2072283 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
2017MarkIII.c
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in7, gyro, sensorGyro)
#pragma config(Sensor, in8, powerExpander, sensorAnalog)
#pragma config(Sensor, dgtl1, claw, sensorDigitalOut)
#pragma config(Sensor, dgtl2, cubeAuton, sensorTouch)
#pragma config(Sensor, dgtl8, leftDriveEnc, sensorQuadEncoder)
#pragma config(Sensor, dgtl10, rightDriveEnc, sensorQuadEncoder)
#pragma config(Sensor, dgtl12, liftStop, sensorTouch)
#pragma config(Sensor, I2C_1, liftEnc, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, rightWheel1, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, leftLift1, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port3, rightLift1, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port4, leftWheel2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, rightWheel2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, rightLift2, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port7, rightLift3, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port8, leftLift2, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port9, leftLift3, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port10, leftWheel1, tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"
int autonomousChoice;
bool debugMode = false;
#include "JonLib/Math.h"
#include "JonLib/Drivebase.h"
#include "JonLib/PID.h"
#include "JonLib/Autonomous.h"
#include "JonLib/LCD.h"
#include "JonLib/Gyro.h"
void setLeftWheelSpeed (int speed) {
motor[leftWheel1] = speed;
motor[leftWheel2] = speed;
}
void setRightWheelSpeed (int speed) {
motor[rightWheel1] = speed;
motor[rightWheel2] = speed;
}
void tankDriveLimit (){
if(abs(vexRT[Ch3])<120){
setLeftWheelSpeed(vexRT[Ch3]);
}
else{
setLeftWheelSpeed(0);
}
if(abs(vexRT[Ch2])<120){
setRightWheelSpeed(vexRT[Ch2]);
}
else{
setRightWheelSpeed(0);
}
}
void lift (int speed, int slew) {
if(abs(speed-motor[leftLift1])>slew) {
if(motor[leftLift1]>speed)
speed = motor[leftLift1]-slew;
else
speed = motor[leftLift1]+slew;
}
motor[leftLift1] = speed;
motor[leftLift2] = speed;
motor[leftLift3] = speed;
motor[rightLift1] = speed;
motor[rightLift2] = speed;
motor[rightLift3] = speed;
}
void liftNoSlew (int speed) {
motor[leftLift1] = speed;
motor[leftLift2] = speed;
motor[leftLift3] = speed;
motor[rightLift1] = speed;
motor[rightLift2] = speed;
motor[rightLift3] = speed;
}
void lift (int speed) {
lift(speed, 15);
}
void init() {
startTask(LCD);
}
void pre_auton () {
init();
}
void resetEncoders() {
SensorValue[leftDriveEnc] = 0;
SensorValue[rightDriveEnc] = 0;
}
void autonomousInit() {
//clear the encoders before start of autonomous
resetEncoders();
//clear drivebase PID targets to prevent robot from running off anywhere
l.target = 0;
r.target = 0;
startTask(drivebasePID);
}
task clawControl () {
while(true) {
if(vexRT[Btn6U]) {
SensorValue[claw] = !SensorValue[claw];
while(vexRT[Btn6U]) { delay(5); }
}
delay(25);
}
}
enum {OPEN = 1, CLOSED = 0};
task autonomous() {
float c = 1.0;
SensorValue[liftEnc] = 0;
//SensorValue[claw] = 0;
//lift(80);
//delay(1200);
//motor[driveLeft] = -127;
//motor[driveRight] = -127;
//delay(1200);
//motor[driveLeft] = 0;
//motor[driveRight] = 0;
//lift(-10);
//SensorValue[claw] = 1;
resetEncoders();
startTask(drivebasePID);
SensorValue[claw] = 1;
delay(1200);
addTarget(1300*c);
delay(1100);
SensorValue[claw] = 0:
while(SensorValue[liftEnc]>-350) {
liftNoSlew(60);
delay(5);
}
liftNoSlew(10);
resetEncoders();
setTarget(-700*c,-700*c);
delay(500);
resetEncoders();
setTarget(-340*c,-2500*c);
delay(400);
resetEncoders();
setTarget(-1000*c,-2000*c);
delay(1300);
resetEncoders();
setTarget(0,0);
stopTask(drivebasePID);
setWheelSpeed(-80,-80);
while(SensorValue[liftEnc]>-600) {
liftNoSlew(127);
delay(5);
}
liftNoSlew(0);
delay(000);
SensorValue[claw] = 1;
delay(700);
//resetEncoders();
//setTarget(0, 200);
//delay(200);
if(true) {
setWheelSpeed(0);
delay(1000);
while(!SensorValue[liftStop]) {
liftNoSlew(-127);
delay(5);
}
liftNoSlew(0);
stopTask(drivebasePID);
setWheelSpeed(127,75);
delay(1500);
SensorValue[claw] = 0;
setWheelSpeed(-127,-75);
delay(500);
while(SensorValue[liftEnc]>-600)
liftNoSlew(127);
liftNoSlew(0);
SensorValue[claw] = 1;
}
if(true) {
while(!SensorValue[liftStop]) {
liftNoSlew(-70);
delay(5);
}
resetEncoders();
startTask(drivebasePID);
setTarget(0,700*c);
delay(1400);
liftNoSlew(0);
resetEncoders();
setTarget(1500*c,1500*c);
delay(1500);
SensorValue[claw] = 0;
delay(200);
while(SensorValue[liftEnc]>-300)
liftNoSlew(60);
liftNoSlew(12);
resetEncoders();
addTarget(-200*c,-600*c);
delay(1000);
stopTask(drivebasePID);
setWheelSpeed(-70);
delay(700);
setWheelSpeed(0);
while(SensorValue[liftEnc]>-600)
liftNoSlew(127);
liftNoSlew(0);
delay(1000);
SensorValue[claw] = 1;
}
stopTask(drivebasePID);
setWheelSpeed(0);
delay(1000);
//while(!SensorValue[liftStop]) {
// liftNoSlew(-127);
// delay(5);
//}
liftNoSlew(0);
}
//pid liftPID;
//void liftControlInit() {
// liftPID.kP = 0.0;
// liftPID.kI = 0.0;
// liftPID.kD = 0.0;
// liftPID.integral = 0.0;
// liftPID.target = SensorValue[liftEnc];
//}
//task liftControl () {
// liftControlInit();
// while(true) {
// liftPID.error = SensorValue[liftEnc] - liftPID.target;
// liftPID.integral = liftPID.integral + liftPID.error;
// if(liftPID.error == 0)
// liftPID.integral = 0;
// if(abs(liftPID.error)>200)
// liftPID.integral = 0;
// liftPID.derivative = liftPID.error - liftPID.lastError;
// liftPID.lastError = liftPID.error;
// lift(liftPID.kP*liftPID.error+liftPID.kI*liftPID.integral+liftPID.kD*liftPID.derivative);
// }
//}
task usercontrol() {
startTask(clawControl);
bool orienting = false;
while(true) {
if(!debugMode) {
tankDrive(vexRT[Ch3], vexRT[Ch2], 15);
//tankDriveLimit();
if(vexRT[Btn5U]) {
lift(127);
} else if (vexRT[Btn5D] && (!SensorValue[liftStop])) {
lift(SensorValue[liftEnc]>-300?-30:-127);
} else if(SensorValue[liftStop]) {
lift(-8);
} else if(vexRT[Btn8D]) {
lift(-127);
} else {
lift(SensorValue[liftEnc]>-500?15:0;
}
}
if(SensorValue[liftStop])
SensorValue[liftEnc] = 0;
if(vexRT[Btn7L])
gyroscope.target = SensorValue[gyro];
if(vexRT[Btn7R] && !orienting){
StartTask(orient);
orienting = true;
} else if(!vexRT[Btn7R] && orienting) {
StopTask(orient);
orienting = false;
}
delay(25);
}
}
Raw
gistfile1.txt
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in7, gyro, sensorGyro)
#pragma config(Sensor, in8, powerExpander, sensorAnalog)
#pragma config(Sensor, dgtl1, claw, sensorDigitalOut)
#pragma config(Sensor, dgtl2, cubeAuton, sensorTouch)
#pragma config(Sensor, dgtl8, leftDriveEnc, sensorQuadEncoder)
#pragma config(Sensor, dgtl10, rightDriveEnc, sensorQuadEncoder)
#pragma config(Sensor, dgtl12, liftStop, sensorTouch)
#pragma config(Sensor, I2C_1, liftEnc, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, rightWheel1, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, leftLift1, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port3, rightLift1, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port4, leftWheel2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, rightWheel2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, rightLift2, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port7, rightLift3, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port8, leftLift2, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port9, leftLift3, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port10, leftWheel1, tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"
int autonomousChoice;
bool debugMode = false;
#include "JonLib/Math.h"
#include "JonLib/Drivebase.h"
#include "JonLib/PID.h"
#include "JonLib/Autonomous.h"
#include "JonLib/LCD.h"
#include "JonLib/Gyro.h"
void setLeftWheelSpeed (int speed) {
motor[leftWheel1] = speed;
motor[leftWheel2] = speed;
}
void setRightWheelSpeed (int speed) {
motor[rightWheel1] = speed;
motor[rightWheel2] = speed;
}
void tankDriveLimit (){
if(abs(vexRT[Ch3])<120){
setLeftWheelSpeed(vexRT[Ch3]);
}
else{
setLeftWheelSpeed(0);
}
if(abs(vexRT[Ch2])<120){
setRightWheelSpeed(vexRT[Ch2]);
}
else{
setRightWheelSpeed(0);
}
}
void lift (int speed, int slew) {
if(abs(speed-motor[leftLift1])>slew) {
if(motor[leftLift1]>speed)
speed = motor[leftLift1]-slew;
else
speed = motor[leftLift1]+slew;
}
motor[leftLift1] = speed;
motor[leftLift2] = speed;
motor[leftLift3] = speed;
motor[rightLift1] = speed;
motor[rightLift2] = speed;
motor[rightLift3] = speed;
}
void liftNoSlew (int speed) {
motor[leftLift1] = speed;
motor[leftLift2] = speed;
motor[leftLift3] = speed;
motor[rightLift1] = speed;
motor[rightLift2] = speed;
motor[rightLift3] = speed;
}
void lift (int speed) {
lift(speed, 15);
}
void init() {
startTask(LCD);
}
void pre_auton () {
init();
}
void resetEncoders() {
SensorValue[leftDriveEnc] = 0;
SensorValue[rightDriveEnc] = 0;
}
void autonomousInit() {
//clear the encoders before start of autonomous
resetEncoders();
//clear drivebase PID targets to prevent robot from running off anywhere
l.target = 0;
r.target = 0;
startTask(drivebasePID);
}
task clawControl () {
while(true) {
if(vexRT[Btn6U]) {
SensorValue[claw] = !SensorValue[claw];
while(vexRT[Btn6U]) { delay(5); }
}
delay(25);
}
}
enum {OPEN = 1, CLOSED = 0};
task autonomous() {
float c = 1.0;
SensorValue[liftEnc] = 0;
//SensorValue[claw] = 0;
//lift(80);
//delay(1200);
//motor[driveLeft] = -127;
//motor[driveRight] = -127;
//delay(1200);
//motor[driveLeft] = 0;
//motor[driveRight] = 0;
//lift(-10);
//SensorValue[claw] = 1;
resetEncoders();
startTask(drivebasePID);
SensorValue[claw] = 1;
delay(1200);
addTarget(1300*c);
delay(1100);
SensorValue[claw] = 0:
while(SensorValue[liftEnc]>-350) {
liftNoSlew(60);
delay(5);
}
liftNoSlew(10);
resetEncoders();
setTarget(-700*c,-700*c);
delay(500);
resetEncoders();
setTarget(-340*c,-2500*c);
delay(400);
resetEncoders();
setTarget(-1000*c,-2000*c);
delay(1300);
resetEncoders();
setTarget(0,0);
stopTask(drivebasePID);
setWheelSpeed(-80,-80);
while(SensorValue[liftEnc]>-600) {
liftNoSlew(127);
delay(5);
}
liftNoSlew(0);
delay(000);
SensorValue[claw] = 1;
delay(700);
//resetEncoders();
//setTarget(0, 200);
//delay(200);
if(true) {
setWheelSpeed(0);
delay(1000);
while(!SensorValue[liftStop]) {
liftNoSlew(-127);
delay(5);
}
liftNoSlew(0);
stopTask(drivebasePID);
setWheelSpeed(127,75);
delay(1500);
SensorValue[claw] = 0;
setWheelSpeed(-127,-75);
delay(500);
while(SensorValue[liftEnc]>-600)
liftNoSlew(127);
liftNoSlew(0);
SensorValue[claw] = 1;
}
if(true) {
while(!SensorValue[liftStop]) {
liftNoSlew(-70);
delay(5);
}
resetEncoders();
startTask(drivebasePID);
setTarget(0,700*c);
delay(1400);
liftNoSlew(0);
resetEncoders();
setTarget(1500*c,1500*c);
delay(1500);
SensorValue[claw] = 0;
delay(200);
while(SensorValue[liftEnc]>-300)
liftNoSlew(60);
liftNoSlew(12);
resetEncoders();
addTarget(-200*c,-600*c);
delay(1000);
stopTask(drivebasePID);
setWheelSpeed(-70);
delay(700);
setWheelSpeed(0);
while(SensorValue[liftEnc]>-600)
liftNoSlew(127);
liftNoSlew(0);
delay(1000);
SensorValue[claw] = 1;
}
stopTask(drivebasePID);
setWheelSpeed(0);
delay(1000);
//while(!SensorValue[liftStop]) {
// liftNoSlew(-127);
// delay(5);
//}
liftNoSlew(0);
}
//pid liftPID;
//void liftControlInit() {
// liftPID.kP = 0.0;
// liftPID.kI = 0.0;
// liftPID.kD = 0.0;
// liftPID.integral = 0.0;
// liftPID.target = SensorValue[liftEnc];
//}
//task liftControl () {
// liftControlInit();
// while(true) {
// liftPID.error = SensorValue[liftEnc] - liftPID.target;
// liftPID.integral = liftPID.integral + liftPID.error;
// if(liftPID.error == 0)
// liftPID.integral = 0;
// if(abs(liftPID.error)>200)
// liftPID.integral = 0;
// liftPID.derivative = liftPID.error - liftPID.lastError;
// liftPID.lastError = liftPID.error;
// lift(liftPID.kP*liftPID.error+liftPID.kI*liftPID.integral+liftPID.kD*liftPID.derivative);
// }
//}
task usercontrol() {
startTask(clawControl);
bool orienting = false;
while(true) {
if(!debugMode) {
tankDrive(vexRT[Ch3], vexRT[Ch2], 15);
//tankDriveLimit();
if(vexRT[Btn5U]) {
lift(127);
} else if (vexRT[Btn5D] && (!SensorValue[liftStop])) {
lift(SensorValue[liftEnc]>-300?-30:-127);
} else if(SensorValue[liftStop]) {
lift(-8);
} else if(vexRT[Btn8D]) {
lift(-127);
} else {
lift(SensorValue[liftEnc]>-500?15:0;
}
}
if(SensorValue[liftStop])
SensorValue[liftEnc] = 0;
if(vexRT[Btn7L])
gyroscope.target = SensorValue[gyro];
if(vexRT[Btn7R] && !orienting){
StartTask(orient);
orienting = true;
} else if(!vexRT[Btn7R] && orienting) {
StopTask(orient);
orienting = false;
}
delay(25);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment