Hello guys.
I need help with a line a of code I wanted my robot to turn right but at the moment I just know how to make it turn to the left.
Go to int onauton_autonomous_0() { To see the problem.
I’m using VEXcode Pro V5.
Any help or inspiration will be appreciated.
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: ForsenCD */
/* Created: Wed Dec 04 2019 */
/* Description: This program will turn right 90 degrees using the */
/* Inertial Sensor. */
/* */
/* */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// LeftMotorBack motor 9
// RightMotorBack motor 10
// Inertial20 inertial 20
// Controller1 controller
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cmath>
using namespace vex;
competition Competition;
// User defined function
void myblockfunction_InertialTurn_heading_velocity_momentum(double myblockfunction_InertialTurn_heading_velocity_momentum__heading, double myblockfunction_InertialTurn_heading_velocity_momentum__velocity, double myblockfunction_InertialTurn_heading_velocity_momentum__momentum);
// User defined function
void myblockfunction_DriveStraight_distance_heading_velocity_Kp(double myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp);
float error, output;
// User defined function
void myblockfunction_InertialTurn_heading_velocity_momentum(double myblockfunction_InertialTurn_heading_velocity_momentum__heading, double myblockfunction_InertialTurn_heading_velocity_momentum__velocity, double myblockfunction_InertialTurn_heading_velocity_momentum__momentum) {
if (myblockfunction_InertialTurn_heading_velocity_momentum__heading > Inertial20.rotation(degrees)) {
while ((myblockfunction_InertialTurn_heading_velocity_momentum__heading - myblockfunction_InertialTurn_heading_velocity_momentum__momentum > Inertial20.rotation(degrees))) {
RightMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
LeftMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
LeftMotorBack.spin(reverse);
RightMotorBack.spin(forward);
wait(5, msec);
}
}
else {
while ((myblockfunction_InertialTurn_heading_velocity_momentum__heading + myblockfunction_InertialTurn_heading_velocity_momentum__momentum < Inertial20.rotation(degrees))) {
LeftMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
RightMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
LeftMotorBack.spin(forward);
RightMotorBack.spin(reverse);
wait(5, msec);
}
}
RightMotorBack.stop();
LeftMotorBack.stop();
}
//My Functions
void myblockfunction_DriveStraight_distance_heading_velocity_Kp(double myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp) {
LeftMotorBack.setRotation(0.0, degrees);
RightMotorBack.setRotation(0.0, degrees);
if (myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity > 0.0) {
while ((LeftMotorBack.rotation(degrees) < myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance)) {
error = myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading - Inertial20.rotation(degrees);
output = error * myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp;
LeftMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity - output), percent);
RightMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity + output), percent);
LeftMotorBack.spin(forward);
RightMotorBack.spin(forward);
wait(5, msec);
}
}
else {
while ((LeftMotorBack.rotation(degrees) > myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance)) {
error = myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading - Inertial20.rotation(degrees);
output = error * myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp;
LeftMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity - output), percent);
RightMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity + output), percent);
LeftMotorBack.spin(forward);
RightMotorBack.spin(forward);
wait(5, msec);
}
}
LeftMotorBack.stop();
RightMotorBack.stop();
}
// "when driver control"
int ondriver_drivercontrol_0() {
double turnImportance = 0.5;
while(1){
double turnVal = Controller1.Axis1.position(percent);
double forwardVal = Controller1.Axis3.position(percent);
double turnVolts = turnVal * 0.12;
double forwardVolts = forwardVal * 0.12 * (1 - (std::abs(turnVolts)/12.0) * turnImportance);
//0 - 12 = -12
//0 + 12 = 12(due to cap)
LeftMotorBack.spin(forward, forwardVolts + turnVolts, voltageUnits::volt);
RightMotorBack.spin(forward, forwardVolts - turnVolts, voltageUnits::volt);
if (Controller1.ButtonR1.pressing()){
IntakeLeft.spin(vex:: directionType::fwd, 85, vex::velocityUnits::pct);
IntakeRight.spin(vex:: directionType::rev, 85, vex::velocityUnits::pct);
}
else if (Controller1.ButtonL1.pressing()) {
IntakeLeft.spin(vex::directionType::rev, 75, vex::velocityUnits::pct);
IntakeRight.spin(vex::directionType::fwd, 75, vex::velocityUnits::pct);
}
else {
IntakeLeft.stop(vex::brakeType::brake);
IntakeRight.stop(vex::brakeType::brake);
}
if(Controller1.ButtonR2.pressing()){
RollerBottomFront.spin(vex::directionType::rev, 95, vex::velocityUnits::pct);
RollerTopBack.spin(vex::directionType::fwd, 95, vex::velocityUnits::pct);
}
else if (Controller1.ButtonL2.pressing()) {
RollerTopBack.spin(vex::directionType::rev, 95, vex::velocityUnits::pct);
}
else {
RollerBottomFront.stop(vex::brakeType::brake);
RollerTopBack.stop(vex::brakeType::brake);
}
if (Controller1.ButtonUp.pressing()){
}
if (Controller1.ButtonX.pressing()){
RollerBottomFront.spin(vex::directionType::fwd,75,vex::velocityUnits::pct);
}
wait(20, msec);
}
return 0;
}
// "when autonomous"
int onauton_autonomous_0() {
Inertial20.startCalibration();
while (Inertial20.isCalibrating()) { task::sleep(50); }
// NOTE Each time the robot turns 90 degrees, we add 90 degrees to the next turn ex. First turn = 90 degrees second turn =180 degrees
// NOTE DriveStraight = (DriveFor, Past Angle, velocity, Kp)
//drives robot 900 ticks forward
myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 0.0, 50.0, 2.0);
//turns the robot 90.0 degrees Left
myblockfunction_InertialTurn_heading_velocity_momentum(90.0, 20.0, 3.0);
//drives robot 900 ticks forward Left
myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 90.0, 50.0, 2.0);
//turns the robot 90.0 degrees Left
myblockfunction_InertialTurn_heading_velocity_momentum(180.0, 20.0, 3.0);
//drives robot 900 ticks forward
myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 180.0, 50, 2.0);
// I want to turn right 90 degrees next time and go backwards how will the next values look after the two actions?
return 0;
}
void VEXcode_driver_task() {
// Start the driver control tasks....
vex::task drive0(ondriver_drivercontrol_0);
while(Competition.isDriverControl() && Competition.isEnabled())
{this_thread::sleep_for(10);}
drive0.stop();
return;
}
void VEXcode_auton_task() {
// Start the auton control tasks....
vex::task auto0(onauton_autonomous_0);
while(Competition.isAutonomous() && Competition.isEnabled())
{this_thread::sleep_for(10);}
auto0.stop();
return;
}
int main() {
vex::competition::bStopTasksBetweenModes = false;
Competition.autonomous(VEXcode_auton_task);
Competition.drivercontrol(VEXcode_driver_task);
}
