I am trying to use the Inertial sensor for autonomous but I have no Ideas on where to start. Anybody have any experience with these sensors? Here is my current code.
#include “vex.h”
using namespace vex;
vex::brain brain;
vex::motor BRD(vex::PORT14,true);
vex::motor FLD(vex::PORT11,false);
vex::motor FRD(vex::PORT15, true);
vex::motor BLD(vex::PORT12, false);
vex::motor motorli(vex::PORT9, false);
vex::motor motorri(vex::PORT2, true);
vex::motor motorramp(vex::PORT16, false);
vex::motor arm(vex::PORT10, false);
vex::controller jcon(controllerType::primary);
// A global instance of competition
competition Competition;
void driveforward(){
BRD.spin(forward);
BLD.spin(forward);
FLD.spin(forward);
FRD.spin(forward);
}
void driveright(){
BRD.spin(forward);
FRD.spin(reverse);
BLD.spin(reverse);
FLD.spin(forward);
}
void driveleft(){
BRD.spin(reverse);
FRD.spin(forward);
FLD.spin(reverse);
BLD.spin(forward);
}
void driveback(){
BRD.spin(reverse);
BLD.spin(reverse);
FLD.spin(reverse);
FRD.spin(reverse);
}
void diagonalleft(){
BLD.spin(forward);
FRD.spin(forward);
}
void diagonalright(){
BRD.spin(forward);
FLD.spin(forward);
}
void diagonalbackright(){
FRD.spin(reverse);
BLD.spin(reverse);
}
void diagonalbackleft(){
FLD.spin(reverse);
BRD.spin(reverse);
}
void drivestop(){
BRD.stop();
FRD.stop();
FLD.stop();
BLD.stop();
}
void turnright(){
FRD.spin(reverse);
BRD.spin(reverse);
BLD.spin(forward);
FLD.spin(forward);
}
void turnleft(){
FRD.spin(forward);
BRD.spin(forward);
BLD.spin(reverse);
FLD.spin(reverse);
}
void raisearm(){
arm.spin(reverse);
}
void lowerarm(){
arm.spin(forward);
}
void armstop(){
arm.stop();
}
void release(){
motorli.spin(reverse);
motorri.spin(reverse);
}
void intake(){
motorri.spin(forward);
motorli.spin(forward);
}
void intakestop(){
motorli.stop();
motorri.stop();
}
void rampup(){
motorramp.spin(reverse);
}
void rampstop(){
motorramp.stop();
}
void rampdown(){
motorramp.spin(forward);
}
void rampspeed(double inputPercentage){
motorramp.setVelocity(inputPercentage,percent);
}
void intakespeed(double inputPercentage){
motorli.setVelocity(inputPercentage,percent);
motorri.setVelocity(inputPercentage,percent);
}
void drivespeed(double inputPercentage){
BLD.setVelocity(inputPercentage,percent);
FLD.setVelocity(inputPercentage,percent);
FRD.setVelocity(inputPercentage,percent);
BRD.setVelocity(inputPercentage,percent);
}
void armspeed(double inputPercentage){
arm.setVelocity(inputPercentage,percent);
}
// define your global instances of motors and other devices here
/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/ /
/ You may want to perform some actions before the competition starts. /
/ Do them in the following function. You must return from this function /
/ or the autonomous and usercontrol tasks will not be started. This /
/ function is only called once after the V5 has been powered on and /
/ not every time that the robot is disabled. /
/---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
}
/---------------------------------------------------------------------------/
/* /
/ Autonomous Task /
/ /
/ This task is used to control your robot during the autonomous phase of /
/ a VEX Competition. /
/ /
/ You must modify the code to add your own robot specific commands here. /
/---------------------------------------------------------------------------*/
void autonomous(void) {
drivespeed(28);
intakespeed(100);
driveforward();
intake();
wait(3000,msec);
drivestop();
intakestop();
drivespeed(50);
driveback();
wait(1400,msec);
drivestop();
turnleft();
wait(725,msec);
drivestop();
// …
// Insert autonomous user code here.
// …
}
/---------------------------------------------------------------------------/
/* /
/ User Control Task /
/ /
/ This task is used to control your robot during the user control phase of /
/ a VEX Competition. /
/ /
/ You must modify the code to add your own robot specific commands here. /
/---------------------------------------------------------------------------*/
void usercontrol(void) {
arm.setVelocity(100,velocityUnits::pct);
motorramp.setVelocity(35,velocityUnits::pct);
motorli.setVelocity(100,velocityUnits::pct);
motorri.setVelocity(100,velocityUnits::pct); // User control code here, inside the loop
while (true) {
if (jcon.ButtonL1.pressing()){
motorri.spin(directionType::fwd);
motorli.spin(directionType::fwd);
}
else if (jcon.ButtonR1.pressing()){
motorri.spin(directionType::rev);
motorli.spin(directionType::rev);
}
else {
motorri.stop(brakeType::hold);
motorli.stop(brakeType::hold);
}
//code that raises arms
if (jcon.ButtonR2.pressing()){
arm.spin(directionType::fwd);
}
else if(jcon.ButtonL2.pressing()){
arm.spin(directionType::rev);
}
else {
arm.stop(brakeType::hold);
}
if(jcon.ButtonB.pressing()){
motorramp.spin(directionType::fwd);
}
else if (jcon.ButtonX.pressing()){
motorramp.spin(directionType::rev);
}
else {motorramp.stop(brakeType::hold);
}
//drive code for Xdrive
BLD.spin(directionType::fwd,jcon.Axis3.value()+jcon.Axis1.value()-jcon.Axis4.value(),velocityUnits::pct);
FLD.spin(directionType::fwd,jcon.Axis3.value()+jcon.Axis1.value()+jcon.Axis4.value(),velocityUnits::pct);
BRD.spin(directionType::fwd,jcon.Axis3.value()-jcon.Axis1.value()+jcon.Axis4.value(),velocityUnits::pct);
FRD.spin(directionType::fwd,jcon.Axis3.value()-jcon.Axis1.value()-jcon.Axis4.value(),velocityUnits::pct);
}
// 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.
// ........................................................................
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}