Programming Inertial Sensor

Programming Support
, ,
1 
#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;
competition Competition = competition();
// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
motor leftMotorA = motor(PORT8, ratio18_1, false);
motor leftMotorB = motor(PORT9, ratio18_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
motor rightMotorA = motor(PORT5, ratio18_1, true);
motor rightMotorB = motor(PORT6, ratio18_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 40, mm, 1);

inertial Inertial13 = inertial(PORT13);

motor Motor3 = motor(PORT3, ratio18_1, false);

controller Controller1 = controller(primary);
motor Motor2 = motor(PORT2, ratio18_1, false);



// generating and setting random seed
void initializeRandomSeed(){
  int systemTime = Brain.Timer.systemHighResolution();
  double batteryCurrent = Brain.Battery.current();
  double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);

  // Combine these values into a single integer
  int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;

  // Set the seed
  srand(seed);
}



void vexcodeInit() {

  //Initializing random seed.
  initializeRandomSeed(); 
}


// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}



// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool Controller1RightShoulderControlMotorsStopped = true;
bool DrivetrainNeedsToBeStopped_Controller1 = true;

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  // process the controller input every 20 milliseconds
  // update the motors based on the input values
  while(true) {
    if(RemoteControlCodeEnabled) {
      
      // calculate the drivetrain motor velocities from the controller joystick axies
      // left = Axis2 + Axis1
      // right = Axis2 - Axis1
      int drivetrainLeftSideSpeed = Controller1.Axis2.position() + Controller1.Axis1.position();
      int drivetrainRightSideSpeed = Controller1.Axis2.position() - Controller1.Axis1.position();
      
      // check if the values are inside of the deadband range
      if (abs(drivetrainLeftSideSpeed) < 5 && abs(drivetrainRightSideSpeed) < 5) {
        // check if the motors have already been stopped
        if (DrivetrainNeedsToBeStopped_Controller1) {
          // stop the drive motors
          LeftDriveSmart.stop();
          RightDriveSmart.stop();
          // tell the code that the motors have been stopped
          DrivetrainNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the motors next time the input is in the deadband range
        DrivetrainNeedsToBeStopped_Controller1 = true;
      }
      
      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainNeedsToBeStopped_Controller1) {
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
        LeftDriveSmart.spin(forward);
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainNeedsToBeStopped_Controller1) {
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
        RightDriveSmart.spin(forward);
      }
      // check the ButtonR1/ButtonR2 status to control Motor3
      if (Controller1.ButtonR1.pressing()) {
        Motor3.spin(forward);
        Controller1RightShoulderControlMotorsStopped = false;
      } else if (Controller1.ButtonR2.pressing()) {
        Motor3.spin(reverse);
        Controller1RightShoulderControlMotorsStopped = false;
      } else if (!Controller1RightShoulderControlMotorsStopped) {
        Motor3.stop();
        // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
        Controller1RightShoulderControlMotorsStopped = true;
      }
    }
    // wait before repeating the process
    wait(20, msec);
  }
  return 0;
}

task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

#pragma endregion VEXcode Generated Robot Configuration

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       {author}                                                  */
/*    Created:      {date}                                                    */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/
#pragma endregion VEXcode Generated Robot Configuration
struct Action {
  enum Type {  // type of our instruction
    WAIT,      // in miliseconds
    FORWARD,        // in meters
    TURN,       // in degrees
    REVERSE, //in meters
    SETROTATION, 
  };
  Type type;
  float value;
  Action(Type t, float v) { type = t; value = v;} // constructor
};

void turn_robot(float rotation) {
  float actualRotation = Inertial13.rotation(degrees);
  float targetRotation = rotation;
  float error = targetRotation - actualRotation;
  float motorSpeed = 0.0;
  float threshold = 2.0;  // Stop when error is within 2 degrees
  float kP = 0.5;         // Proportional gain (adjust as needed for your robot)
  float minSpeed = 8;
  // Loop until we reach the target heading
  while (fabs(error) > threshold) {
    actualRotation = Inertial13.rotation(degrees);
    error = targetRotation - actualRotation;
  if (fabs(error) < threshold) {
      RightDriveSmart.stop(brake);
      LeftDriveSmart.stop(brake);
      break;
    }
    motorSpeed = kP * error;  // Proportional speed (positive for right, negative for left)
    // Cap speed to prevent excessive turning (adjust min/max as needed)
    if (motorSpeed > 60.0) motorSpeed = 60.0;
    if (motorSpeed < -60.0) motorSpeed = -60.0;
    if (fabs(motorSpeed) < minSpeed)
      motorSpeed = (motorSpeed > 0 ? minSpeed : -minSpeed);
    // Set direction and speed
    if (motorSpeed > 0) {
      // Turn right
      RightDriveSmart.setVelocity(motorSpeed, pct);
      LeftDriveSmart.setVelocity(motorSpeed, pct);
      RightDriveSmart.spin(forward);
      LeftDriveSmart.spin(reverse);
    } else if (motorSpeed < 0) {
      // Turn left
      RightDriveSmart.setVelocity(motorSpeed, pct);
      LeftDriveSmart.setVelocity(motorSpeed, pct);
      RightDriveSmart.spin(reverse);
      LeftDriveSmart.spin(forward);
    } else {
      // Stop if error is zero (though threshold should handle this
      RightDriveSmart.stop(brake);
      LeftDriveSmart.stop(brake);
      break;
    }
    // Small wait for sensor update and control loop
    wait(20, msec);
  }
} 

void execute_autonomous_instruction(Action instruction) { // we process all the actions here
  switch (instruction.type) {
    case Action::WAIT :
        wait(instruction.value, msec);
        break;
    case Action::FORWARD: 
        Drivetrain.setDriveVelocity(75, percent); 
        Drivetrain.driveFor(forward, instruction.value, mm);
        Drivetrain.stop();
        break; 
    case Action::TURN:
        turn_robot(instruction.value);
        Drivetrain.stop();
        break;
    case Action::REVERSE:
        Drivetrain.setDriveVelocity(75, percent);    
        Drivetrain.driveFor(reverse, instruction.value, mm);
        Drivetrain.stop();
        break;
    case Action::SETROTATION:
        Inertial13.setRotation(instruction.value, degrees);
        break;
}

  
}
void autonomous(void) {
  // Reset sensors if needed
  RemoteControlCodeEnabled = false;
  Inertial13.setRotation(0, degrees);

  Action autonomous_actions[] = {
    Action(Action::TURN, 90.f),
    Action(Action::WAIT, 50),
    Action(Action::SETROTATION, 0),
    Action(Action::TURN, -90.f),
    };

  for(Action action : autonomous_actions) { 
    execute_autonomous_instruction(action);
  }
}
void usercontrol(void) {
  // Enable the remote control task
  RemoteControlCodeEnabled = true;

  while (1) {
    // Keep this loop open so the background task continues to run
    wait(20, msec);
  }
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  vexcodeInit();
  
  // Begin project code
  Inertial13.calibrate();
  while (Inertial13.isCalibrating()) {
    wait(2000, msec);
  }

  // Stop the drivetrain once target is reached
  Drivetrain.stop();
  

  return 0;
}

I have been trying to program inertial sensor for weeks. This is my recent code. I have tried ai’s and they also add many things. But still it just makes our robot spin to the right for one time, stop nearly a second and turn endlessly. We don’t think there is an issue with the position of inertial. Do you have any suggestions?

2

It seems as though you are switching which way the motors turn when you are on the other side of the target. This would work if motor speed remained positive but since it is calculated using the error which can become negative when it turns too far, reversing the motors (ie. Right reverse Left forward->Left reverse right forward) is actually making the robot to continue turning farther instead of turing back toward the target position. The behavior you described would make sense, it is slowing down as it approaches the target but then turning slightly too far and then continuing to turn farther from the target. As the error continues to increase it just spins faster in an endless loop.

3

thanks for your response. you’re right, our robot absloutely does the same thing you mentioned. well i am really new to this vex coding so can you tell me what should i change or add to this code exactly? thank you a lot again

4

In your auton function under the comment “set motor direction and speed” you need to remove either the turn right or turn left section so that it turns the right way. You also shouldn’t need to stop the motors if motor speed is zero because that is handled by the threshold.

5 
void turn_robot(float rotation) {
  float actualRotation = Inertial13.rotation(degrees);
  float targetRotation = rotation;
  float error = targetRotation - actualRotation;
  float motorSpeed = 0.0;
  float threshold = 2.0;  // Stop when error is within 2 degrees
  float kP = 0.5;         // Proportional gain (adjust as needed for your robot)
  float minSpeed = 8;
  // Loop until we reach the target heading
  while (fabs(error) > threshold) {
    actualRotation = Inertial13.rotation(degrees);
    error = targetRotation - actualRotation;
  if (fabs(error) < threshold) {
      RightDriveSmart.stop(brake);
      LeftDriveSmart.stop(brake);
      break;
    }
    motorSpeed = kP * error;  // Proportional speed (positive for right, negative for left)
    // Cap speed to prevent excessive turning (adjust min/max as needed)
    if (motorSpeed > 60.0) motorSpeed = 60.0;
    if (motorSpeed < -60.0) motorSpeed = -60.0;
    if (fabs(motorSpeed) < minSpeed)
      motorSpeed = (motorSpeed > 0 ? minSpeed : -minSpeed);
    // Set direction and speed
    if (motorSpeed > 0) {
      // Turn right
      RightDriveSmart.setVelocity(motorSpeed, pct);
      LeftDriveSmart.setVelocity(motorSpeed, pct);
      RightDriveSmart.spin(forward);
      LeftDriveSmart.spin(reverse);
  }
    // Small wait for sensor update and control loop
    wait(20, msec);
  }
}

I have changed it like that but it didn’t make robot turn as i want. when i tried for 90 degrees it turned more than 100 degrees.

6

You need to remove the “ if (motorSpeed > 0)”. Right now once the robot drive past the target the motor speed would be negative so it is not spinning the motors back the other way to correct for this. If yo7 don’t like this oscillation you could lower your kP so it doesn’t overshoot as far.

7

could you explain what you meant? our all turn function is in this “if” function. if we remove it i dont think the robot will turn. also i have an another question. when we try the code, i dont think robot can use the kP value. it should get slower when the target is closer, but it doesnt…

8

Remove the if statement but not he things inside it.