BaseDrive/BaseDriveV1.c at master · team62/BaseDrive · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
#pragma config(Motor,  port1,           rightWheel1,   tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           rightWheel2,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           rightWheel3,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           leftWheel2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           leftWheel3,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           topShooter,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bottomShooter, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          leftWheel1,    tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

void pre_auton() {
  bStopTasksBetweenModes = true;

}

task autonomous() {

}

task usercontrol() {
  while (true) {

    //tank drive
    //left wheels
    if(abs(vexRT(Ch3))<10) {
      motor[leftWheel1] = 0;
      motor[leftWheel2] = 0;
      motor[leftWheel3] = 0;
    } else {
      motor[leftWheel1] = vexRT(Ch3);
      motor[leftWheel2] = vexRT(Ch3);
      motor[leftWheel3] = vexRT(Ch3);
    }
    //left wheels
    if(abs(vexRT(Ch2))<10) {
      motor[rightWheel1] = 0;
      motor[rightWheel2] = 0;
      motor[rightWheel3] = 0;
    } else {
      motor[rightWheel1] = vexRT(Ch2);
      motor[rightWheel2] = vexRT(Ch2);
      motor[rightWheel3] = vexRT(Ch2);
    }

    //shooting mech
    if(vexRT(Btn6U)) {
      motor[topShooter] = 127;
      motor[bottomShooter] = -127;
    }

    //intake for shooting mech
    if(vexRT(Btn5U)) {
      motor[intake] = 127;
    }

    //Anywhere from 25-50 Msec pause
    wait1Msec(30);
  }
}