ProsCode/lionFunctions.cpp at master · BuddySwan/ProsCode · 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
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
#include "lmain.h"

pros::Motor leftF(LeftF);
pros::Motor leftB(LeftB);
pros::Motor rightF(RightF,true);
pros::Motor rightB(RightB,true);
pros::Motor catapult(Cata);
pros::Motor catapult2(Cata2);
pros::Motor flipper(Flipper);
pros::Controller master (CONTROLLER_MASTER);
pros::Motor roller(Roller);
pros::Motor uptake(Uptake);
pros::Motor pokeBar(PokeBar);

bool rollToggle = true;
bool flipperDown = true;
bool flipToggle = true;
bool upToggle = true;
bool uptakeOn = false;
int power = 127;
void Roll(){
    if(master.get_digital(DIGITAL_Y) && rollToggle){
        rollToggle = false;
        power = -power;
        roller.move(power);
    }else if(!master.get_digital(DIGITAL_Y)){
      rollToggle = true;
    }
}

void waitUntil(){
  while(leftF.get_position() != leftF.get_target_position() && leftB.get_position() != leftB.get_target_position() && rightF.get_position() != rightF.get_target_position() && rightB.get_position() != rightB.get_target_position()){
      pros::delay(2);
  }
}

void move(double distance){
  double speed = 100;
  leftF.move_relative(distance,speed);
  leftB.move_relative(distance,speed);
  rightF.move_relative(distance,speed);
  rightB.move_relative(distance,speed);
  waitUntil();
}

 void moveTiles(int numTiles){
    int tileLength = 20;
    move(numTiles*tileLength);
 }
 void turnDegrees(int angle){
    int rightAngle = 750;
    int speed = 100;
    leftF.move_relative(angle*rightAngle/90,speed);
    leftB.move_relative(angle*rightAngle/90,speed);
    rightF.move_relative(-angle*rightAngle/90,speed);
    rightB.move_relative(-angle*rightAngle/90,speed);
    waitUntil();
 }

void flipFlip(){
  if(master.get_digital(DIGITAL_X) && flipToggle){
    flipperDown = !flipperDown;
    flipper.move_relative(flipperDown * 150, 100);
    flipToggle = false;
  }else if(!master.get_digital(DIGITAL_X)){
    flipToggle = true;
  }
}

 void drive(){
   int power = master.get_analog(ANALOG_LEFT_Y);
   int turn = master.get_analog(ANALOG_RIGHT_X);
   int left = power + turn;
   int right = power - turn;
   leftF.move(left);
   leftB.move(left);
   rightF.move(right);
   rightB.move(right);
 }
 void lift(bool auton){
    if((master.get_digital(DIGITAL_B) && upToggle) ){
      upToggle = false;
      uptakeOn = !uptakeOn;
      uptake.move(127*uptakeOn);
    }else if(!master.get_digital(DIGITAL_B)){
      upToggle = true;
    }
 }
 void runCatapult(){
   int speed = 100;
   if(master.get_digital(DIGITAL_R1)){
     catapult.move(speed);
     catapult2.move(speed);
     pros::lcd::set_text(1, "R1");
   }
   if(master.get_digital(DIGITAL_R2)){
     catapult.move(-speed);
     catapult2.move(-speed);
     pros::lcd::set_text(1, "R2");
   }
 }
 void shoot(){

 }