I am having trouble programming Marcos for an auto stack program, and I also need some ideas on how to for the lift to go up with our tray at the same time. Below is my driver control program:
void usercontrol(void) {
// User control code here, inside the loop
while (1) {
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
ChassisControl();
// 吸方塊
if(Controller1.ButtonR1.pressing()){
SuckL.spin(directionType::fwd, 200, velocityUnits::rpm);
SuckR.spin(directionType::rev, 200, velocityUnits::rpm);
}
else if (Controller1.ButtonR1.pressing() || Controller1.ButtonR2.pressing() == false){
SuckL.stop(brakeType::hold);
SuckR.stop(brakeType::hold);
}
// 吐方塊
if (Controller1.ButtonR2.pressing()) {
SuckL.spin(directionType::fwd, -200, velocityUnits::rpm);
SuckR.spin(directionType::rev, -200, velocityUnits::rpm);
}
// tray push (stack)
else if (Controller1.ButtonX.pressing()) {
Tray.spin(directionType::rev, 40, velocityUnits::rpm);
}
else if(Controller1.ButtonUp.pressing()){
Tray.spin(directionType::rev, 80, velocityUnits::rpm);
}
// tray return
else if (Controller1.ButtonB.pressing() && LimitSwitchB.pressing() == false) {
Tray.spin(directionType::fwd, 80, velocityUnits::rpm);
} else if (Controller1.ButtonA.pressing() == false){
Tray.stop(brakeType::hold);
}
else if (Controller1.ButtonA.pressing()){
if(suckSwitch == 1){
Tray.rotateFor(-300, rotationUnits::deg, 30, velocityUnits::pct, true);
Tray.setStopping(hold);
Tray.rotateFor(-200, rotationUnits::deg, 15, velocityUnits::pct, true);
Tray.setStopping(hold);
suckSwitch = 0;
}
}
// lift up
if (Controller1.ButtonL1.pressing()) {
Lift.spin(directionType::fwd, -200, velocityUnits::rpm);
}
// lift down
else if (Controller1.ButtonL2.pressing()) {
Lift.spin(directionType::fwd, 200, velocityUnits::rpm);
} else {
Lift.stop(brakeType::hold);
}
}
wait(100, msec);
// ........................................................................
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
The auto stack program is at the bottom that is toggled with button A. There is also an int suckSwitch at the very top. Any advice and constructive criticisms will be very much appreciated. Thanks!