shark/mobilerobotics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Update 'main.c'

Browse Source
This commit is contained in:
Cole Deck 2019-04-03 17:22:56 -05:00
parent d3031766aa
commit f72cd3546c
1 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
main.c
View File

@ -1,11 +1,11 @@
#pragma config(I2C_Usage, I2C1, i2cSensors) #pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, , tmotorVex393_HBridge, openLoop) #pragma config(Motor, port1, , tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, shoot, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port2, shoot, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1) #pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2) #pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop) #pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop) #pragma config(Motor, port8, , tmotorVex393_MC29, openLoop)
@ -37,7 +37,7 @@ void shootBall() {
wait(1.25); // Shooting takes 1.25 seconds. wait(1.25); // Shooting takes 1.25 seconds.
motor[shoot] = STOP; motor[shoot] = STOP;
} }
void clearEnc();
void joystickDrive() { void joystickDrive() {
if(abs(vexRT[Ch3]) > DEADZONE) { if(abs(vexRT[Ch3]) > DEADZONE) {
motor[driveLB] = vexRT[Ch3]; motor[driveLB] = vexRT[Ch3];
@ -82,46 +82,50 @@ void pre_auton() {
bStopTasksBetweenModes = true; bStopTasksBetweenModes = true;
} }
void driveTiles(int numberOfTiles, boolean direction) { void driveTiles(int numberOfTiles, bool direction) {
clearEnc();
while(direction && numberOfTiles * TILE > nMotorEncoder[1]) [ while(direction && numberOfTiles * TILE > nMotorEncoder[1]) {
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
motor[driveLB] = 100; motor[driveLB] = 100;
motor[driveLF] = 100; motor[driveLF] = 100;
motor[driveRB] = 90; motor[driveRB] = 90;
motor[driveRF] = 90; motor[driveRF] = 90;
} }
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
motor[driveLB] = 90; motor[driveLB] = 90;
motor[driveLF] = 90; motor[driveLF] = 90;
motor[driveRB] = 100; motor[driveRB] = 100;
motor[driveRF] = 100; motor[driveRF] = 100;
} else { } else {
motor[driveLB] = 100; motor[driveLB] = 100;
motor[driveLF] = 100; motor[driveLF] = 100;
motor[driveRB] = 100; motor[driveRB] = 100;
motor[driveRF] = 100; motor[driveRF] = 100;
} }
} }
while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) [ while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) {
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
motor[driveLB] = -100; motor[driveLB] = -100;
motor[driveLF] = -100; motor[driveLF] = -100;
motor[driveRB] = -90; motor[driveRB] = -90;
motor[driveRF] = -90; motor[driveRF] = -90;
} }
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
motor[driveLB] = -90; motor[driveLB] = -90;
motor[driveLF] = -90; motor[driveLF] = -90;
motor[driveRB] = -100; motor[driveRB] = -100;
motor[driveRF] = -100; motor[driveRF] = -100;
} else { } else {
motor[driveLB] = -100; motor[driveLB] = -100;
motor[driveLF] = -100; motor[driveLF] = -100;
motor[driveRB] = -100; motor[driveRB] = -100;
motor[driveRF] = -100; motor[driveRF] = -100;
} }
} }
motor[driveLB] = 0;
motor[driveLF] = 0;
motor[driveRB] = 0;
motor[driveRF] = 0;
} }
task autonomous() { task autonomous() {
shootBall(); shootBall();
@ -129,10 +133,14 @@ task autonomous() {
//driveTiles(1, false); //driveTiles(1, false);
} }
void turnRight(int turns) { void turnRight(int turns) {
while(turns * 360 < nMotorEncoder[1] while(turns * 360 < nMotorEncoder[1]){
}
} }
void turnLeft(int turns) { void turnLeft(int turns) {
while(turns * 360 < nMotorEncoder[1]){
}
} }
void clearEnc() { void clearEnc() {