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 | } |