View difference between Paste ID: i6YjBR4W and Yxu1p8v9
SHOW: | | - or go back to the newest paste.
1
#pragma region VEXcode Generated Robot Configuration
2
// Make sure all required headers are included.
3
#include <stdio.h>
4
#include <stdlib.h>
5
#include <stdbool.h>
6
#include <math.h>
7
#include <string.h>
8
#include <iostream>
9
 
10
float axis1 = 0.0;
11
float axis3 = 0.0;
12
13
#include "vex.h"
14-
competition Competition;
14+
15
using namespace vex;
16
 
17
// Brain should be defined by default
18
brain Brain;
19
 
20
 
21
// START V5 MACROS
22
#define waitUntil(condition)                                                   \
23
  do {                                                                         \
24
    wait(5, msec);                                                             \
25
  } while (!(condition))
26
 
27
#define repeat(iterations)                                                     \
28
  for (int iterator = 0; iterator < iterations; iterator++)
29
// END V5 MACROS
30
 
31
 
32
// Robot configuration code.
33-
distance Distance1 = distance(PORT7);
33+
34
motor leftMotorA = motor(PORT14, ratio6_1, false);
35
motor leftMotorB = motor(PORT11, ratio6_1, false);
36
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
37
motor rightMotorA = motor(PORT17, ratio6_1, true);
38
motor rightMotorB = motor(PORT19, ratio6_1, true);
39
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
40
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 219.44, 295, 40, mm, 1);
41
 
42
motor cataMotorA = motor(PORT8, ratio36_1, false);
43
motor cataMotorB = motor(PORT3, ratio36_1, true);
44
motor_group cata = motor_group(cataMotorA, cataMotorB);
45
 
46
motor MotorGroup7MotorA = motor(PORT4, ratio36_1, true);
47
motor MotorGroup7MotorB = motor(PORT9, ratio36_1, false);
48
motor_group MotorGroup7 = motor_group(MotorGroup7MotorA, MotorGroup7MotorB);
49
 
50
digital_out DigitalOutB = digital_out(Brain.ThreeWirePort.B);
51
digital_out DigitalOutD = digital_out(Brain.ThreeWirePort.D);
52
 
53
 
54
 
55
// define variable for remote controller enable/disable
56
bool RemoteControlCodeEnabled = true;
57
// define variables used for controlling motors based on controller inputs
58
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
59
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
60
 
61
// define a task that will handle monitoring inputs from Controller1
62
int rc_auto_loop_function_Controller1() {
63
  // process the controller input every 20 milliseconds
64
  // update the motors based on the input values
65
  while(true) {
66
    if(RemoteControlCodeEnabled) {
67
 
68
      // calculate the drivetrain motor velocities from the controller joystick axies
69
      // left = Axis3 + Axis1
70
      // right = Axis3 - Axis1
71
      double y = 0;
72-
double x = Controller1.Axis3.position();
72+
double x = axis3;
73
 
74
x = x * 1.28;
75
 
76
x = fabs(x);
77
 
78
if(0<=x&&x<60){
79
    y = pow(1.04029, x * 1.284467);
80
}else if(60<=x&&x<80){
81
    y = 0.75 * x -25;
82
}else if(80<=x&&x<=108){
83
    y = 1.0357 * x - 47.85714857;
84
}else if(x > 108){
85
    y = 63 + pow(1.232099, x - 108);
86
}
87
 
88
y = y/1.28;
89
                 
90-
if(Controller1.Axis3.position() < 0){
90+
if(axis3 < 0){
91
    y = -y;
92
}
93
double sy = 0;
94-
double sx = Controller1.Axis1.position();
94+
double sx = axis1;
95
 
96
sx = sx * 1.28;
97
 
98
sx = fabs(sx);
99
 
100
if(0<=sx&&sx<60){
101
    sy = pow(1.04029, sx * 1.284467);
102
}else if(60<=sx&&sx<80){
103
    sy = 0.75 * sx -25;
104
}else if(80<=sx&&sx<=108){
105
    sy = 1.0357 * sx - 47.85714857;
106
}else if(sx > 108){
107
    sy = 63 + pow(1.232099, sx - 108);
108
}
109
 
110
sy = sy/1.28;
111
                 
112-
if(Controller1.Axis1.position() < 0){
112+
if(axis1 < 0){
113
    sy = -sy;
114
}
115
 
116
y = -y;
117
 
118
      int drivetrainLeftSideSpeed = y + sy;
119
      int drivetrainRightSideSpeed = y - sy;
120
 
121
      // check if the value is inside of the deadband range
122
      if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
123
        // check if the left motor has already been stopped
124
        if (DrivetrainLNeedsToBeStopped_Controller1) {
125
          // stop the left drive motor
126
          LeftDriveSmart.stop();
127
          // tell the code that the left motor has been stopped
128
          DrivetrainLNeedsToBeStopped_Controller1 = false;
129
        }
130
      } else {
131
        // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
132
        DrivetrainLNeedsToBeStopped_Controller1 = true;
133
      }
134
      // check if the value is inside of the deadband range
135
      if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
136
        // check if the right motor has already been stopped
137
        if (DrivetrainRNeedsToBeStopped_Controller1) {
138
          // stop the right drive motor
139
          RightDriveSmart.stop();
140
          // tell the code that the right motor has been stopped
141
          DrivetrainRNeedsToBeStopped_Controller1 = false;
142
        }
143
      } else {
144
        // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
145
        DrivetrainRNeedsToBeStopped_Controller1 = true;
146
      }
147
 
148
      // only tell the left drive motor to spin if the values are not in the deadband range
149
      if (DrivetrainLNeedsToBeStopped_Controller1) {
150
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
151
        LeftDriveSmart.spin(forward);
152
      }
153
      // only tell the right drive motor to spin if the values are not in the deadband range
154
      if (DrivetrainRNeedsToBeStopped_Controller1) {
155
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
156
        RightDriveSmart.spin(forward);
157
      }
158
    }
159
    // wait before repeating the process
160
    wait(20, msec);
161
  }
162
  return 0;
163
}
164
 
165
 
166
task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
167
 
168
#pragma endregion VEXcode Generated Robot Configuration
169
 
170
 
171
// Include the V5 Library
172
#include "vex.h"
173
  
174
// Allows for easier use of the VEX Library
175
using namespace vex;
176
 
177
float myVariable, no;
178
 
179
// "when started" hat block
180
int whenStarted1() {
181
  Drivetrain.setDriveVelocity(600.0, rpm);
182
  Drivetrain.setTurnVelocity(100.0, percent);
183
  MotorGroup7.setMaxTorque(100, percent);
184
  MotorGroup7.setVelocity(100, percent);
185
  cata.setMaxTorque(100, percent);
186
  cata.setVelocity(100, percent);
187
  bool wingState = false;
188
  bool digitalB1 = false;
189-
  bool ytoggle = false;
189+
190
  bool pl = false;
191
  float catapos = 0;
192-
  //read controller
192+
193-
  int counter = 0;
193+
194-
  int tempA = 0;
194+
//array read code
195-
  int tempB = 0;
195+
196-
  int tempL1 = 0;
196+
197-
  int tempR1 = 0;
197+
198-
  int tempU = 0;
198+
//array setup
199-
  int tempD = 0;
199+
float values[] = {};
200-
  int tempR = 0;
200+
201-
  int tempL = 0;
201+
//printf("%fn", values[0]);
202-
  float temp3 = 0.0;
202+
203-
  float temp1 = 0.0;
203+
204-
  bool matchload = false;
204+
205-
  int i = 0;
205+
//int code
206
int ck = 0;
207
 
208
//the y-int for the algebra
209-
    if(Controller1.ButtonR1.pressing()){
209+
int mathA = -10;
210-
      if(Controller1.Axis3.position() < -15.0){
210+
int mathB = -9;
211
int mathL1 = -8;
212
int mathR1 = -7;
213
int mathU = -6;
214
int mathD = -5;
215
int mathR = -4;
216
int mathL = -3;
217
int math3 = -2;
218
int math1 = -1;
219-
    if(Controller1.ButtonDown.pressing()){
219+
220
float outA = 0.0;
221
float outB = 0.0;
222-
    if(Controller1.ButtonUp.pressing()){
222+
float outL1 = 0.0;
223
float outR1 = 0.0;
224
float outU = 0.0;
225
float outD = 0.0;
226
float outR = 0.0;
227
float outL = 0.0;
228
float out3 = 0.0;
229
float out1 = 0.0;
230-
    if(Controller1.ButtonL1.pressing()){
230+
231
bool buttonA = false;
232
bool buttonB = false;
233-
    if (Controller1.ButtonL1.pressing()){
233+
bool buttonL1 = false;
234
bool buttonR1 = false;
235
bool buttonUp = false;
236
bool buttonDown = false;
237
bool buttonRight = false;
238
bool buttonLeft = false;
239
240-
    if(Controller1.ButtonLeft.pressing()){
240+
241
    
242
    if(ck >= 44){
243
        break;
244
    }
245
    
246
    //regular code goes here
247
    //make sure to replace Controller1.ButtonA.pressing() with “buttonA”…
248
 
249
    //math at the start before reading the values
250
    mathA = mathA + 10;
251
    mathB = mathB + 10;
252
    mathL1 = mathL1 + 10;
253-
         ytoggle = !ytoggle;
253+
    mathR1 = mathR1 + 10;
254
    mathU = mathU + 10;
255-
     if(ytoggle == true){
255+
    mathD = mathD + 10;
256-
       //DigitalOutB.set(true);
256+
    mathR = mathR + 10;
257
    mathL = mathL + 10;
258-
       //DigitalOutB.set(false);
258+
    math3 = math3 + 10;
259
    math1 = math1 + 10;
260
    
261-
  if(Controller1.ButtonA.pressing()){
261+
    //read the values off of the array and set the values accordingly
262-
      matchload = !matchload;
262+
    outA = values[mathA];
263-
      wait(350, msec);
263+
    outB = values[mathB];
264
    outL1 = values[mathL1];
265-
  if(matchload == true){
265+
    outR1 = values[mathR1];
266-
    DigitalOutB.set(true);
266+
    outU =  values[mathU];
267-
    MotorGroup7.stop();
267+
    outD = values[mathD];
268-
    printf("%.1f\n", (Distance1.objectDistance(inches)));
268+
    outR = values[mathR];
269-
    if(i == 0){
269+
    outL = values[mathL];
270-
      cata.spinFor(forward, 318, degrees);
270+
    out3 = values[math3];
271-
      ++i;
271+
    out1 = values[math1];
272
    
273-
    if(Distance1.objectDistance(inches) < 1.5){
273+
    //do stuff on the controller
274
    if(outA == 1){
275-
      cata.spinFor(forward, 1, turns);
275+
        buttonA = true;
276
    } else {
277-
  } else{
277+
        buttonA = false;
278-
    DigitalOutB.set(false);
278+
279-
    MotorGroup7.setVelocity(100, percent);
279+
    if(outB == 1){
280-
    i = 0;
280+
        buttonB = true;
281-
    if(i == 1){
281+
282-
      cata.spinFor(reverse, 318, degrees);
282+
        buttonB = false;
283-
  }}
283+
284
    if(outL1 == 1){
285-
  wait(5, msec);
285+
        buttonL1 = true;
286
    } else {
287
        buttonL1 = false;
288
    }
289
    if(outR1 == 1){
290-
int onauton_autonomous_0() {
290+
        buttonR1 = true;
291-
  wait(15.0, seconds);
291+
292
        buttonR1 = false;
293
    }
294
    if(outU == 1){
295-
int ondriver_drivercontrol_0() {
295+
        buttonUp = true;
296
    } else {
297
        buttonUp = false;
298
    }
299
    if(outD == 1){
300-
void VEXcode_driver_task() {
300+
        buttonDown = true;
301-
  // Start the driver control tasks....
301+
302-
  vex::task drive0(ondriver_drivercontrol_0);
302+
        buttonDown = false;
303-
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
303+
304-
  drive0.stop();
304+
    if(outL == 1){
305-
  return;
305+
        buttonLeft = true;
306
    } else {
307
        buttonLeft = false;
308-
void VEXcode_auton_task() {
308+
309-
  // Start the auton control tasks....
309+
    float axis3 = out3;
310-
  vex::task auto0(onauton_autonomous_0);
310+
    float axis1 = out1;
311-
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
311+
              double y = 0;
312-
  auto0.stop();
312+
double x = axis3;
313-
  return;
313+
314
x = x * 1.28;
315
 
316
x = fabs(x);
317
 
318
if(0<=x&&x<60){
319
    y = pow(1.04029, x * 1.284467);
320
}else if(60<=x&&x<80){
321
    y = 0.75 * x -25;
322
}else if(80<=x&&x<=108){
323-
  vex::competition::bStopTasksBetweenModes = false;
323+
324-
  Competition.autonomous(VEXcode_auton_task);
324+
325-
  Competition.drivercontrol(VEXcode_driver_task);
325+
326
}
327
 
328
y = y/1.28;
329
                 
330
if(axis3 < 0){
331
    y = -y;
332
}
333
double sy = 0;
334
double sx = axis1;
335
 
336
sx = sx * 1.28;
337
 
338
sx = fabs(sx);
339
 
340
if(0<=sx&&sx<60){
341
    sy = pow(1.04029, sx * 1.284467);
342
}else if(60<=sx&&sx<80){
343
    sy = 0.75 * sx -25;
344
}else if(80<=sx&&sx<=108){
345
    sy = 1.0357 * sx - 47.85714857;
346
}else if(sx > 108){
347
    sy = 63 + pow(1.232099, sx - 108);
348
}
349
 
350
sy = sy/1.28;
351
                 
352
if(axis1 < 0){
353
    sy = -sy;
354
}
355
 
356
y = -y;
357
 
358
      RightDriveSmart.setVelocity(y - sy, percent);
359
      LeftDriveSmart.setVelocity(y + sy, percent);
360
      RightDriveSmart.spin(forward);
361
      LeftDriveSmart.spin(forward);
362
    
363
    ++ck;
364
  axis3 = out3;
365
  axis1 = out1;
366
    cata.setStopping(hold);
367
    cata.stop();
368
    if(buttonR1 == true){
369
      if(axis3 < -15.0){
370
        MotorGroup7.spin(reverse);
371
      } else {
372
        MotorGroup7.spin(forward);
373
      }
374
    } else {
375
      MotorGroup7.spin(forward);
376
    }
377
    turng = floor(cata.position(turns));
378
    if(buttonDown == true){
379
      cata.spin(reverse);
380
    }
381
    if(buttonUp == true){
382
      cata.spin(forward);
383
    }
384
    if(catapos <= 60 || catapos >= 330){
385
      digitalB2 = false;
386
    } else {
387
      digitalB2 = true;
388
    }
389
    if(buttonA == true){
390
      cata.spinFor(forward, 360, degrees);
391
      digitalB1 = true;
392
      wait(250, msec);
393
    } else{
394
      digitalB1 = false;
395
    }
396
    if(buttonL1 == true){
397
      wingState = !wingState;
398
    }
399
    if (wingState == true){
400
      DigitalOutD.set(true);
401
      wait(150, msec);
402
    } else{
403
      DigitalOutD.set(false);
404
      wait(150, msec);
405
    }
406
    if(buttonLeft == true){
407
      pl = !pl;
408
      wait(150, msec);
409
    }
410
    catapos = cata.position(degrees) - (turng * 360);
411
    //if(pl == true){
412
      //DigitalOutB.set(true);
413
    //} else if((digitalB1 == true || digitalB2 == true)||(digitalB1 == true & digitalB2 == true)){
414
        //DigitalOutB.set(true);
415
      //} else {
416
        //DigitalOutB.set(false);
417
      //}
418
     if(Controller1.ButtonY.pressing()){
419
         DigitalOutB.set(true);
420
     }else{
421
         DigitalOutB.set(false);
422
     }
423
     LeftDriveSmart.spin(forward);
424
     printf("%f\t", outA);
425
     printf("%f\t", outB);
426
     printf("%f\t", outL1);
427
     printf("%f\t", outR1);
428
     printf("%f\t", outU);
429
     printf("%f\t", outD);
430
     printf("%f\t", outL);
431
     printf("%f\t", outR);
432
     printf("%f\t", out1);
433
     printf("%f\n", out3);
434
435
    wait(5, msec);
436
  }
437
  
438
  return 0;
439
}
440
 
441
 
442
int main() {
443
  // post event registration
444
 
445
  // set default print color to black
446
 
447
  // wait for rotation sensor to fully initialize
448
  wait(30, msec);
449
 
450
  whenStarted1();
451
}