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-10 19:14:09 -05:00
parent 52471a17a5
commit 092a2cbbe3
1 changed files with 52 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
main.c
View File

@@ -19,9 +19,10 @@
// some variable definitions // some variable definitions
#define MAX_SPEED 127 // Max speed of the motors #define MAX_SPEED 127 // Max speed of the motors
#define MAX_AUTO_SPEED 100 // Max speed of the driving motors during the autonomous code
#define STOP 0 #define STOP 0
#define DEADZONE 10 #define DEADZONE 10
#define DRIVE_OFFSET 10
/* /*
23.4 / (2 * pi * 2.075) * 672.2 23.4 / (2 * pi * 2.075) * 672.2
23.4 = inches in a tile 23.4 = inches in a tile
@@ -39,6 +40,13 @@
#define FORWARD true #define FORWARD true
#define REVERSE false #define REVERSE false
void stopDriving() { // Stop all driving motors
motor[driveLB] = STOP;
motor[driveLF] = STOP;
motor[driveRB] = STOP;
motor[driveRF] = STOP;
}
void clearEnc() { // Reset driving motor encoder values to 0 void clearEnc() { // Reset driving motor encoder values to 0
nMotorEncoder[driveRB] = 0; nMotorEncoder[driveRB] = 0;
nMotorEncoder[driveLB] = 0; nMotorEncoder[driveLB] = 0;
@@ -53,28 +61,22 @@ void shootBall() {
void turntoRight(float turns) { void turntoRight(float turns) {
clearEnc(); clearEnc();
while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){ while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
motor[driveLB] = 100; motor[driveLB] = MAX_AUTO_SPEED;
motor[driveLF] = 100; motor[driveLF] = MAX_AUTO_SPEED;
motor[driveRB] = -100; motor[driveRB] = -MAX_AUTO_SPEED;
motor[driveRF] = -100; motor[driveRF] = -MAX_AUTO_SPEED;
} }
motor[driveLB] = 0; stopDriving();
motor[driveLF] = 0;
motor[driveRB] = 0;
motor[driveRF] = 0;
} }
void turntoLeft(float turns) { void turntoLeft(float turns) {
clearEnc(); clearEnc();
while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){ while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
motor[driveLB] = -100; motor[driveLB] = -MAX_AUTO_SPEED;
motor[driveLF] = -100; motor[driveLF] = -MAX_AUTO_SPEED;
motor[driveRB] = 100; motor[driveRB] = MAX_AUTO_SPEED;
motor[driveRF] = 100; motor[driveRF] = MAX_AUTO_SPEED;
} }
motor[driveLB] = 0; stopDriving();
motor[driveLF] = 0;
motor[driveRB] = 0;
motor[driveRF] = 0;
} }
@@ -82,7 +84,7 @@ void flipOn() {
motor[bintake] = -MAX_SPEED; motor[bintake] = -MAX_SPEED;
} }
void ballOff() { void ballOff() {
motor[bintake] = 0; motor[bintake] = STOP;
} }
void ballIn() { void ballIn() {
motor[bintake] = MAX_SPEED; motor[bintake] = MAX_SPEED;
@@ -135,48 +137,47 @@ void driveTiles(float numberOfTiles, bool direction) {
// when direction is true, move forward, otherwise go in reverse // when direction is true, move forward, otherwise go in reverse
clearEnc(); clearEnc();
while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) { while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) {
if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) { if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
motor[driveLB] = 100; motor[driveLB] = MAX_AUTO_SPEED;
motor[driveLF] = 100; motor[driveLF] = MAX_AUTO_SPEED;
motor[driveRB] = 90; motor[driveRB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
motor[driveRF] = 90; motor[driveRF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
} }
if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveRB]) { if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveRB]) {
motor[driveLB] = 90; motor[driveLB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
motor[driveLF] = 90; motor[driveLF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
motor[driveRB] = 100; motor[driveRB] = MAX_AUTO_SPEED;
motor[driveRF] = 100; motor[driveRF] = MAX_AUTO_SPEED;
} else { } else {
motor[driveLB] = 100; motor[driveLB] = MAX_AUTO_SPEED;
motor[driveLF] = 100; motor[driveLF] = MAX_AUTO_SPEED;
motor[driveRB] = 100; motor[driveRB] = MAX_AUTO_SPEED;
motor[driveRF] = 100; motor[driveRF] = MAX_AUTO_SPEED;
} }
} }
while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) { while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) {
if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) { if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
motor[driveLB] = -100; motor[driveLB] = -MAX_AUTO_SPEED;
motor[driveLF] = -100; motor[driveLF] = -MAX_AUTO_SPEED;
motor[driveRB] = -90; motor[driveRB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
motor[driveRF] = -90; motor[driveRF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
} }
if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveLB]) { if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
motor[driveLB] = -90; motor[driveLB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
motor[driveLF] = -90; motor[driveLF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
motor[driveRB] = -100; motor[driveRB] = -MAX_AUTO_SPEED;
motor[driveRF] = -100; motor[driveRF] = -MAX_AUTO_SPEED;
} else { } else {
motor[driveLB] = -100; motor[driveLB] = -MAX_AUTO_SPEED;
motor[driveLF] = -100; motor[driveLF] = -MAX_AUTO_SPEED;
motor[driveRB] = -100; motor[driveRB] = -MAX_AUTO_SPEED;
motor[driveRF] = -100; motor[driveRF] = -MAX_AUTO_SPEED;
} }
} }
motor[driveLB] = 0; stopDriving();
motor[driveLF] = 0;
motor[driveRB] = 0;
motor[driveRF] = 0;
} }
task autonomous() { task autonomous() {
turntoRight(0.03); turntoRight(0.03);
shootBall(); shootBall();
@@ -207,8 +208,7 @@ task autonomous() {
ballOff(); ballOff();
shootBall(); shootBall();
driveTiles(0.05, REVERSE); driveTiles(0.05, REVERSE);
driveTiles(0.33, FORWARD); // Hit middle column bottom flag driveTiles(0.33, FORWARD);
//driveTiles(2, REVERSE);
wait(2); wait(2);
turntoRight(1); turntoRight(1);
driveTiles(2.2, REVERSE); driveTiles(2.2, REVERSE);