Last active
December 4, 2017 10:34
-
-
Save Genora51/df6d865345ebb01f1eed8d90aff6e223 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(I2C_Usage, I2C1, i2cSensors) | |
#pragma config(Sensor, I2C_1, armEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign) | |
#pragma config(Motor, port1, rightFrontMotor, tmotorVex393_HBridge, openLoop) | |
#pragma config(Motor, port2, leftFrontMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port3, rightBackMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port4, rightArmMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port5, leftIntakeMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port6, rightIntakeMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port7, clawLiftMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port8, leftArmMotor, tmotorVex393_MC29, openLoop, encoderPort, I2C_1) | |
#pragma config(Motor, port9, clawMotor, tmotorVex393_MC29, openLoop) | |
#pragma config(Motor, port10, leftBackMotor, tmotorVex393_HBridge, openLoop) | |
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// | |
#pragma platform(VEX) | |
//Competition Control and Duration Settings | |
#pragma competitionControl(Competition) | |
#pragma autonomousDuration(20) | |
#pragma userControlDuration(120) | |
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify! | |
///////////////////////////////////////////////////////////////////////////////////////// | |
// | |
// Pre-Autonomous Functions | |
// | |
// You may want to perform some actions before the competition starts. Do them in the | |
// following function. | |
// | |
///////////////////////////////////////////////////////////////////////////////////////// | |
void pre_auton() | |
{ | |
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between | |
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false. | |
bStopTasksBetweenModes = true; | |
// 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. | |
// ..................................................................................... | |
AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code. | |
} | |
///////////////////////////////////////////////////////////////////////////////////////// | |
// | |
// 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. | |
// | |
///////////////////////////////////////////////////////////////////////////////////////// | |
task usercontrol() | |
{ | |
// PID CONSTANTS | |
float pid_Kp = 0.5; | |
float pid_Ki = 0.04; | |
float pid_Kd = 0.04; | |
// PID Values | |
int targetPos = 0; | |
bool offRun = true; | |
nMotorEncoder[leftArmMotor] = 0; | |
int error; | |
int prevError; | |
int error_diff; | |
int error_sum; | |
while (true) | |
{ | |
int fd = vexRT[Ch3]; // Forward Joystick | |
int sd = vexRT[Ch4]; // Side Joystick | |
int tr = vexRT[Ch1]; // Turn Joystick | |
int lfv = fd + sd + tr; // Left front motor | |
int rfv = -fd + sd + tr; // Right front motor | |
int lbv = fd - sd + tr; // Left back motor | |
int rbv = -fd - sd + tr; // Right back motor | |
motor[leftFrontMotor] = -lfv; | |
motor[rightFrontMotor] = -rfv; | |
motor[rightBackMotor] = -rbv; | |
motor[leftBackMotor] = -lbv; | |
// Intake | |
armControl(leftIntakeMotor, Btn7D, Btn7U, -127); | |
armControl(rightIntakeMotor, Btn7D, Btn7U, 127); | |
// Claw | |
armControl(clawMotor, Btn6D, Btn6U, 60); | |
// Arm | |
int armSpeed = 80; | |
if(vexRT[Btn5U]) | |
{ | |
offRun = true; | |
targetPos = nMotorEncoder[leftArmMotor]; | |
setMotor(leftArmMotor, armSpeed); | |
} | |
else if(vexRT[Btn5D]) | |
{ | |
offRun = true; | |
targetPos = nMotorEncoder[leftArmMotor]; | |
setMotor(leftArmMotor, -armSpeed); | |
} | |
else | |
{ | |
if (offRun) { | |
offRun = false; | |
error_sum = 0; | |
prevError = 0; | |
setMotor(leftArmMotor, 0); | |
} | |
error = targetPos - nMotorEncoder[leftArmMotor]; | |
error_sum += error; | |
error_diff = error - prevError; | |
motor[leftArmMotor] = (error_sum * pid_Ki) + (error * pid_Kp) + (error_diff * pid_Kd); | |
prevError = error; | |
} | |
motor[rightArmMotor] = -motor[leftArmMotor]; | |
armControl(clawLiftMotor, Btn8U, Btn8D, 90); | |
wait1Msec(25); | |
} | |
} |
changes to nMotorEncoder[...]. will look up if this will work
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Wouldn't that mean that if it reached the target, it would stop even if the button was still held? Is there instead a way to make sure that corrections are relative to the initial speed?
Also, you'll need to change
nMotorEncoder(...)
tonMotorEncoder[...]
because for some reason it's an array or something.