Why is this giving me a Memory Permission error

Programming Support
1

This code which is supposed to read floats from a file and store them into a vector is giving me a memory permission error:

void readFile(const char* fileName, std::vector<float> *vect) {

  //std::vector<float> vect;
  //Open the stream for reading in binary
  std::ifstream inputFile(fileName, std::ios::in | std::ios::binary);
  //Check if the file succesfully opened to ensure no errors
  if (inputFile){
    // Determine the file length
    //Goes to the end of the file
    inputFile.seekg(0, std::ios_base::end);
    //Gives the current position in the file in terms of bytes
    std::size_t size = inputFile.tellg();
    //Returns back to the beginning
    inputFile.seekg(0, std::ios_base::beg);  

    //For testing purposes, Prints the number of bytes read
    Brain.Screen.setCursor(3, 1);
    Brain.Screen.print(size);

    //Resize vector to store the data with the correct number of spaces
    //Must divide by size of float (4 bytes) as that is the raw data type stored in the binary file
    vect -> resize(size/sizeof(float));

    //Load the data onto the vector array
    inputFile.read((char*) &vect[0], size);
    //Close the file
    inputFile.close();
  }else{
    //Let the user know about failed file opening
    Brain.Screen.setCursor(2, 1);
    Brain.Screen.print("Failed to open file");
  }
}
2

Check the size that’s coming out of your resize calculation, and make sure that it’s coming out to a multiple of 4. It’s the weekend and almost 10pm, and I’m tired, so I’m not mathing as well as I could, but it’s very possible you’re not allocating all the space you need to store all the values.

Also, as I think on it, if you’re reading float values as text from a file, you’ll need to scanf them in so that they actually get converted to numbers. Otherwise, you’re storing the ASCII characters, not the numbers.

1 Like
3

Okay, so re-reading on my computer (I have to stop looking at code when I’m on my phone…), I see you’re working with this in binary, so you can ignore my second paragraph.

I’m going to go back to my initial guess on size is coming out as one less storage spot than what you need, and here’s why.

  1. Let’s say you have 8 values of float stored in binary in the file. float is 4 bytes, so that should be 32 bytes in the file.
  2. The first byte is at memory location 0 in the file, the final byte value will be at position 31. Note that this is 32 bytes total still, since we start counting with zero.
  3. Compiler math will dictate for anything other than float or double, values are truncated, never rounded.
  4. So, 31/4 == 7.75 => 7. Your vector has now been resized to accommodate 7 values, not the 8 that you need.

So, change:

vect->resize(size/sizeof(float));

to

vect -> resize((size/sizeof(float)) + 1); // Add one to account for starting counting at zero

And I’ll put money on you’re no longer getting the error.

Stack overflows will make you pull your hair out, be happy in this case that Vex has put some nice protection around the user space programs to intercept these things and present them to you, rather than just having the microcontroller reset mid-program and REALLY forcing you to do some fun debugging… :smiley:

2 Likes
4

One last followup to this, since it’s otherwise going to bug me and someone else reading this might also squirrel this away in their brain. In this sort of case, especially when maybe you don’t know how many values to expect, you can always test a result with the modulo (%) operator to see if there is a remainder.

In this case:

if((size % sizeof(float)) != 0)
{
  size += 1;
}

Which accounts for “I forgot we start counting at zero” errors, but this is probably the best way to do it in total:

   //Gives the current position in the file in terms of bytes
    std::size_t size = inputFile.tellg();
    size += (sizeof(float) - (size % sizeof(float));

In this case, if size comes out to a multiple of 4 already, then the % comes out to zero, and it adds zero. If size comes in with a value that leaves a remainder of 1/4, 1/2 (2/4), or 3/4, that added line automatically pads out to give you an even memory boundary. Your last value is still going to be trash, and so you probably should add some sanity checking to your numbers (if you can), but you avoid the risk of crashing due to a memory overflow like what you look to be hitting now.

1 Like
5

Thank you for the advice, I will probably add this to prevent further issues later on. Despite this, this was not the solution. I tried playing around with it and debugging and I think the problem is the vector array.

I originally used a global vector and that worked perfectly, but when I tried to switch to a more modular readFile function that would allow me to read any file into any vector I encountered this issue. I made sure to isolate the problem and it is definitly coming from this function.

The memory permision error is 0304BF94 if that helps. I realize it is late at night so I don’t expect a soon response. Thank you

6

Additionally, this is where I am calling the method:

//Vector to store the settings read in from the file
std::vector currSettings;

readFile(“settings.dat”, &currSettings);

(This is in the Pre-Auton section)

7

You’re defining it outside the scope of the function, which is good. Having it completely global is not likely to fix the issue.

Is std::vector currSettings;

the actual declaration, or is it

std::vector<float> currSettings;

I would think you’re likely declaring it with the <float> option, or I expect the compiler would throw an error. Maybe it’s throwing a warning if you’re using the first instance, and you just weren’t noticing the warning? I would think it wouldn’t like a casting to a different type (assuming it’s defaulting to something like <int> without an explicit declaration).

Have you tried getting it working with a single value, and just statically sizing the vector? That helps establish that you’re reading properly and getting the values you expect. Also, to verify, the values being written to the file are definitely binary, right? If you open the file with notepad or something similar, it’s not human-readable, correct? If it’s human readable, then you’re accessing the data improperly and need to take a different approach.

Posting your complete code and an example input file (as a .zip if necessary) probably would be the most helpful from a browsability standpoint. As to the memory permission error number itself, I don’t have enough insight as to whether that’s a specific error code or a memory address (I suspect it’s memory address).

1 Like
8

Thank you so much for your help. I think I figured it out. In my:

inputFile.read((char*) &vect[0], size);

I used &vect[0] which I think when using a pointer led it treat the vect as an array of vectors and attempt to grab the vector at position 0. This is wrong, so I changed it to:

inputFile.read((char*) &(*vect)[0], size);

This defrences the pointer and allows it to point to the correct vector. I’m not sure why this would give me a memory error (perhaps it is the fact that is is trying to point to a vector that doesn’t exist), but this seemed to fix the problem. I am reading the correct data.

Please let me know if you see a problem with this solution.

Here is the entirety of my code if anyone wanted to look through. I am still working on commenting the new parts as it is quite long so be warned:

//Setting up environment
#include "vex.h"
#include <math.h>
#include <fstream>
#include <vector>
#include <sys/stat.h>
#include <functional>
#include <list>
using namespace std;
using namespace vex;

//A global instance of competition
competition Competition;

//Constant variables that allow for changing the bot's behaviour
const float movementTimeDelay = 20; //Milliseconds
const float recordingTimeDelay = 20; //Milliseconds
const float allowedHeadingError = 30; //Degrees
const float disallowHeadingErrorWhenOffset = 5; //Degrees
const float desiredPIDError = 1; //Degrees, the PID correction will run until the error is within this range of the desired value
const uint32_t borderWidth = 5; //Pixels, the border size around each button on the brain screen
const int brainScreenHeight = 240; //Pixels
const int brainScreenWidth = 480; //Pixels
const int brainPrintHeight = 15; //Rows
const int brainPrintWidth = 48; //Cols

//Changeable settings through the brain (coded buttons)
int catapultSpeed = 50; //RPM
const char* slots[5] = {"slot1.dat", "slot2.dat", "slot3.dat", "slot4.dat", "slot5.dat"}; int slotIndex = -1; //Slot/file to read and write the recorded movement
bool PIDActive = true; //Acvtivates the PID correction during autonomous

std::vector<float> settingsVector;

//Holds all the motors
std::vector<vex::motor> motors = {PORT9, PORT8, PORT3, PORT1, PORT10, PORT2, PORT4, PORT6};
//Front_Left, Front_Right, Back_Left, Back_Right, Middle_Left, Middle_Right, Catapult_Right, Catapult_Left

//Global vector for storing recorded movement
std::vector<float> recordedMovementVector;


/*---------------------------------------------------------------------------*/
/*                                 Button Class                              */
/*                                                                           */
/*    A self made class that allows on screen buttons to be created easily   */
/*    Takes in a function and will run it whenever the button is pressed     */
/* Through optimizing it can usually read a 75,000 byte (1 minute autonomous)*/
/*                       in roughly 50 milliseconds                          */
/*---------------------------------------------------------------------------*/
class Button{
  int height;
  int width;
  int xPos;
  int yPos;
  int textX;
  int textY;
  const char* label;
  vex::color buttonColor;
  std::function<void()> func;
  public:
    Button(int height, int width, int xPos, int yPos, int textX, int textY, std::function<void()> func, vex::color buttonColor, const char* label){
      this -> height = height;
      this -> width = width;
      this -> xPos = xPos;
      this -> yPos = yPos;
      this -> textX = textX;
      this -> textY = textY;
      this -> buttonColor = buttonColor;
      this -> func = func;
      this -> label = label;
    }
    Button(){
      xPos = 0;
      yPos = 0;
      height = brainScreenHeight;
      width = brainScreenWidth;
    }

    void drawButton(){
      Brain.Screen.setFillColor(buttonColor);
      Brain.Screen.setPenWidth(borderWidth);
      Brain.Screen.setPenColor(black);
      Brain.Screen.drawRectangle(xPos, yPos, width, height);
      Brain.Screen.setCursor(textY + 2, textX + 2);
      Brain.Screen.print(label);
    }
    void screenPressed(int xPosPressed, int yPosPressed){
      if (xPosPressed > xPos && xPosPressed < xPos + width && yPosPressed > yPos && yPosPressed < yPos + height){
        func();
      }
    }
};

/*---------------------------------------------------------------------------*/
/*                              Button Grid Class                            */
/*                                                                           */
/*    A self made class that allows on screen buttons to be created easily   */
/*    Takes in a function and will run it whenever the button is pressed     */
/* Through optimizing it can usually read a 75,000 byte (1 minute autonomous)*/
/*                       in roughly 50 milliseconds                          */
/*---------------------------------------------------------------------------*/
class ButtonGrid{
  int heights;
  int widths;
  int rows;
  int cols;
  std::list<Button> buttons;
  int numButtons = 0;
  public:
    ButtonGrid(int rows, int cols){
      heights = brainScreenHeight / rows;
      widths = brainScreenWidth / cols;
      this -> rows = rows;
      this -> cols = cols;
    }

    bool addButton(std::function<void()> func, vex::color buttonColor, const char* label){
      if (numButtons >= rows * cols){
        return false;
      }
      Button newButton(heights, widths, fmod(numButtons, cols) * widths, (numButtons / cols) * heights, fmod(numButtons, cols) * (brainPrintWidth/cols), (numButtons / cols) * (brainPrintHeight/rows), func, buttonColor, label);
      buttons.insert(buttons.end(), newButton);
      numButtons++;
      return true;
    }
    void drawGrid(){
      for (Button i : buttons){
        i.drawButton();
      }
    }
    void screenPressed(int xPosPressed, int yPosPressed){
      for (Button i : buttons){
        i.screenPressed(xPosPressed, yPosPressed);
      }
    }
};



/*---------------------------------------------------------------------------*/
/*                          Read File Function                               */
/*                                                                           */
/*        Able to read a binary file of raw floats into the given            */
/*       vector for use with recording and playing back movements            */
/* Through optimizing it can usually read a 75,000 byte (1 minute autonomous)*/
/*                       in roughly 50 milliseconds                          */
/*---------------------------------------------------------------------------*/
void readFile(const char* fileName, std::vector<float> *vect) {
  //Open the stream for reading in binary
  std::ifstream inputFile(fileName, std::ios::in | std::ios::binary);
  //Check if the file succesfully opened to ensure no errors
  if (inputFile){
    // Determine the file length
    //Goes to the end of the file
    inputFile.seekg(0, std::ios_base::end);
    //Gives the current position in the file in terms of bytes
    std::size_t size = inputFile.tellg();
    if (size % sizeof(float) > 0){
      size += (sizeof(float) - (size % sizeof(float)));
    }
    //Returns back to the beginning
    inputFile.seekg(0, std::ios_base::beg);  

    //For testing purposes, Prints the number of bytes read
    Brain.Screen.setCursor(3, 1);
    Brain.Screen.print(size);

    //Resize vector to store the data with the correct number of spaces
    //Must divide by size of float (4 bytes) as that is the raw data type stored in the binary file
    vect -> resize(size/sizeof(float));
    //Load the data onto the vector array
    inputFile.read((char*) &(*vect)[0], size);
    //Close the file
    inputFile.close();
  }else{
    //Let the user know about failed file opening
    Brain.Screen.setCursor(2, 1);
    Brain.Screen.print("Failed to open file");
  }
}

/*---------------------------------------------------------------------------*/
/*                          Write To File Function                           */
/*                                                                           */
/*      Able to write to a binary file in the form of raw signed floats      */
/* This binary file is saved to a sd-card to allow for saving of the movement*/
/*       even if the brain is turned off or code unexpectedly terminates     */
/* By writing to a binary file instead of a .txt, we can compress the amount */
/* of space needed due to the fact that a text fyle requires 1 byte for each */
/*       character including periods (decimals) and negative signs           */
/*---------------------------------------------------------------------------*/
void writeToFile(const char* fileName, std::vector<float> outputVector) {
  //Make or open a binary file for writing
  ofstream output(fileName, std::ios::out | std::ios::binary);
  //Write the vector into the binary file using signed raw floats in order to allow for negatives and quick reading
  //The amount of bytes stored is equal to the number of floats times the size of each of float in bytes (4 bytes)
  output.write((char*)&outputVector[0], outputVector.size() * sizeof(float));
  //Close the file
  output.close();
}

/*---------------------------------------------------------------------------*/
/*                              Button Functions                             */
/*                                                                           */
/*                       Records the movement of the bot                     */
/*     and saves it to a binary file on the sd card for playback later       */
/*---------------------------------------------------------------------------*/
void selectSlot1(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now reading and writing from slot: 1");
  slotIndex = 0;
  wait(3, seconds);
}
void selectSlot2(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now reading and writing from slot: 2");
  slotIndex = 1;
  wait(3, seconds);
}
void selectSlot3(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now reading and writing from slot: 3");
  slotIndex = 2;
  wait(3, seconds);
}
void selectSlot4(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now reading and writing from slot: 4");
  slotIndex = 3;
  wait(3, seconds);
}
void selectSlot5(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now reading and writing from slot: 5");
  slotIndex = 4;
  wait(3, seconds);
}
void selectDefaultSlot(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  Brain.Screen.print("Now using default autonomous");
  slotIndex = -1;
  wait(3, seconds);
}

void selectSlot(){
  while(Brain.Screen.pressing()){
    wait(5, msec);
  }
  Brain.Screen.clearScreen();
  ButtonGrid slotButtons(3, 2);
  slotButtons.addButton(selectSlot1, white, "Slot 1");
  slotButtons.addButton(selectSlot2, white, "Slot 2");
  slotButtons.addButton(selectSlot3, white, "Slot 3");
  slotButtons.addButton(selectSlot4, white, "Slot 4");
  slotButtons.addButton(selectSlot5, white, "Slot 5");
  slotButtons.addButton(selectDefaultSlot, white, "Default");
  slotButtons.drawGrid();
  while(!Brain.Screen.pressing()){
    wait(5, msec);
  }
  slotButtons.screenPressed(Brain.Screen.xPosition(), Brain.Screen.yPosition());
}
void increaseCataRPM(){
  catapultSpeed++;
  Brain.Screen.setCursor(8, 22);
  Brain.Screen.print("Catapult RPM: ");
  Brain.Screen.print(catapultSpeed);
}
void decreaseCataRPM(){
  catapultSpeed--;
  Brain.Screen.setCursor(8, 22);
  Brain.Screen.print("Catapult RPM: ");
  Brain.Screen.print(catapultSpeed);
}
bool settingCataRPM = false;
void exitCataRPMSetting(){
  settingCataRPM = false;
}
void setCatapultRPM(){
  Brain.Screen.clearScreen();
  ButtonGrid changeCataButtons(2, 2);
  changeCataButtons.addButton(decreaseCataRPM, red, "Decrease RPM");
  changeCataButtons.addButton(increaseCataRPM, green, "Increase RPM");
  changeCataButtons.addButton(exitCataRPMSetting, white, "Exit");
  changeCataButtons.drawGrid();
  Brain.Screen.setCursor(8, 22);
  Brain.Screen.print("Catapult RPM: ");
  Brain.Screen.print(catapultSpeed);
  settingCataRPM = true;
  while(settingCataRPM){
    if (Brain.Screen.pressing()){
      changeCataButtons.screenPressed(Brain.Screen.xPosition(), Brain.Screen.yPosition());
      wait(40, msec);
    }
    wait(5, msec);
  }
}
void togglePID(){
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
  PIDActive = !PIDActive;
  Brain.Screen.print("PID correction: ");
  Brain.Screen.print(PIDActive ? "ENABLED" : "DISABLED");
  wait(3, seconds);
}
bool settingsMenuActive = false;
void saveSettings(){
  Brain.Screen.clearScreen();
  vector<float> settings = {static_cast<float>(catapultSpeed), static_cast<float>(PIDActive), static_cast<float>(slotIndex)};
  writeToFile("settings.dat", settings);
  settingsMenuActive = false;
  wait(3, seconds);
}

/*---------------------------------------------------------------------------*/
/*                          Settings Menu Function                           */
/*                                                                           */
/*                       Records the movement of the bot                     */
/*     and saves it to a binary file on the sd card for playback later       */
/*---------------------------------------------------------------------------*/
void settingsMenu(){
  while (settingsMenuActive){
    while(Brain.Screen.pressing()){
      wait(5, msec);
    }
    Brain.Screen.clearScreen();
    ButtonGrid settingsButtons(2, 2);
    settingsButtons.addButton(selectSlot, yellow, "Reading/Writing Slots");
    settingsButtons.addButton(setCatapultRPM, red, "Catapult RPM");
    settingsButtons.addButton(togglePID, blue, "Toggle PID Control");
    settingsButtons.addButton(saveSettings, green, "Save Settings");
    settingsButtons.drawGrid();
    while(!Brain.Screen.pressing()){
      wait(5, msec);
    }
    settingsButtons.screenPressed(Brain.Screen.xPosition(), Brain.Screen.yPosition());
    Brain.Screen.clearScreen();
  }
}

/*---------------------------------------------------------------------------*/
/*                          Toggle Settings Menu Function                    */
/*                                                                           */
/*                       Records the movement of the bot                     */
/*     and saves it to a binary file on the sd card for playback later       */
/*---------------------------------------------------------------------------*/
void toggleSettingsMenu(){
  //If not already recording then start recording
  if (!settingsMenuActive){
    //Toggle the settings menu, automatically turns off after going through
    settingsMenuActive = true;
    settingsMenu();
  }
}

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Function                          */
/*                                                                           */
/*              Actions performed before the competition starts.             */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
  //Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  //Calibrating the inertial sensor, threaded task so other pre auton code can still run
  //2 second calibration
  inertialSensor.calibrate(2);

  //Set the stopping types for each motor
  Catapult_Left.setStopping(vex::hold);
  Catapult_Right.setStopping(vex::hold);
  Front_Left.setStopping(vex::coast);
  Front_Right.setStopping(vex::coast);
  Middle_Left.setStopping(vex::coast);
  Middle_Right.setStopping(vex::coast);
  Back_Left.setStopping(vex::coast);
  Back_Right.setStopping(vex::coast);

  //For testing purposes
  Brain.Screen.setCursor(2, 1);
  Brain.Screen.print("Running pre auton");

  //For optimization testing, stores the start time of reading the file for later uses
  long loadStartTime = Brain.Timer.time(msec);

  //Wait until done calibrating
  while (inertialSensor.isCalibrating()){
  }

  //Set the initial heading to 0 after calibration
  inertialSensor.setHeading(0, vex::degrees);

  //Vector to store the settings read in from the file
  std::vector<float> currSettings;

  readFile("settings.dat", &currSettings);
  if (currSettings.size() >= 3){
    Brain.Screen.setCursor(4, 1);
    Brain.Screen.print("hi");
    vector<float>::iterator settingsIter = currSettings.begin();
    catapultSpeed = static_cast<int>(*settingsIter);
    settingsIter++;
    PIDActive = static_cast<bool>(*settingsIter);
    settingsIter++;
    slotIndex = static_cast<int>(*settingsIter);
    Brain.Screen.print("Cata RPM: ");
    Brain.Screen.print(catapultSpeed);
    Brain.Screen.print(", Slot: ");
    Brain.Screen.print(slotIndex + 1);
    Brain.Screen.print(", PID: ");
    Brain.Screen.print(PIDActive);
  }

  //For optimization testing purposes
  Brain.Screen.setCursor(7, 1);
  Brain.Screen.print("Finished pre auton, took time(msec): ");
  Brain.Screen.print(Brain.Timer.time(msec) - loadStartTime);
}

/*---------------------------------------------------------------------------*/
/*                     Correct Turning Error Function                        */
/*                                                                           */
/*      Able to write to a binary file in the form of raw signed floats      */
/* This binary file is saved to a sd-card to allow for saving of the movement*/
/*       even if the brain is turned off or code unexpectedly terminates     */
/* By writing to a binary file instead of a .txt, we can compress the amount */
/* of space needed due to the fact that a text fyle requires 1 byte for each */
/*       character including periods (decimals) and negative signs           */
/*---------------------------------------------------------------------------*/
void correctTurningError(double desiredValue) {
  //Tuning values for the PID algorithm
  //Tuning for pontential
  float kP = 3;
  //Tuning for integral
  float kI = 0.0000005;
  //Tuning for derivative
  float kD = 5;
  
  //Needed instance variables
  float error = inertialSensor.heading(vex::degrees) - desiredValue; //Current error (degrees from desired value) - P
  float prevError = 0; //Previous calculated error
  float derivative; //Uses derivative ideology to slow down the turn if the system (robot) is approaching the desired value too fast - D
  float integralError = 0; //Uses inegral ideology to speed up the system (robot) if it is taking too long to reach the desired value - I

  //Continue running the PID algorithm until the absolute error is less than the desired error
  while (fabs(error) > desiredPIDError){
    //P - Potential: Speeds the turn up when far away from the desired value, 
    //but slows it down as it gets closer
    error = inertialSensor.heading(vex::degrees) - desiredValue; 
    //Finds the minumum angle by wrapping back around if the calculated difference angle is too big
    if (error > 180){
      error -= 360;
    }else if(error < -180){
      error += 360;
    }
    //Gets the difference between the previous and cuurent errors. This ensures that this value will increase as the turn speeds up
    //This corrects for overshooting of the Potential value and Integral values
    derivative = error - prevError; //Derivative
    //Sums up all errors to account for a consistent non-zero error (also known as a steady-state error)
    //This corrects for the Potential not spinning fast enough
    integralError += error;//Integral

    //Sum up each value times their respective tuning constants to get the motor velocities (RPM)
    float motorVelocity = error * kP + integralError * kI + derivative * kD;

    //Set the drivetrain motor's velocities
    Front_Left.setVelocity(fabs(motorVelocity), rpm);
    Front_Right.setVelocity(fabs(motorVelocity), rpm);;
    Back_Left.setVelocity(fabs(motorVelocity), rpm);;
    Back_Right.setVelocity(fabs(motorVelocity), rpm);;
    Middle_Left.setVelocity(fabs(motorVelocity), rpm);;
    Middle_Right.setVelocity(fabs(motorVelocity), rpm);;

    if (motorVelocity < 0){
      //Spin each drivetrain motor in the correct direction for turning right
      Front_Left.spin(vex::forward);
      Front_Right.spin(vex::reverse);
      Back_Left.spin(vex::forward);
      Back_Right.spin(vex::reverse);
      Middle_Left.spin(vex::forward);
      Middle_Right.spin(vex::reverse);
    }else{
      //Spin each drivetrain motor in the correct direction for turning left
      Front_Left.spin(vex::reverse);
      Front_Right.spin(vex::forward);
      Back_Left.spin(vex::reverse);
      Back_Right.spin(vex::forward);
      Middle_Left.spin(vex::reverse);
      Middle_Right.spin(vex::forward);
    }

    //Setup previous error for the next loop
    prevError = error;

    //Print information to the screen for troubleshooting
    Brain.Screen.setCursor(5, 1);
    Brain.Screen.print(error);
    Brain.Screen.setCursor(6, 1);
    Brain.Screen.print(inertialSensor.heading(vex::degrees));

    //Time delay
    wait(20, msec);
  }
  //Stop the motors when the PID is done
  Front_Left.setVelocity(0, rpm);
  Front_Right.setVelocity(0, rpm);
  Back_Left.setVelocity(0, rpm);
  Back_Right.setVelocity(0, rpm);
  Middle_Left.setVelocity(0, rpm);
  Middle_Right.setVelocity(0, rpm);
}

/*---------------------------------------------------------------------------*/
/*                          Record Movement Function                         */
/*                                                                           */
/*                       Records the movement of the bot                     */
/*     and saves it to a binary file on the sd card for playback later       */
/*---------------------------------------------------------------------------*/
//Variable to keep track of recording status
bool recording = false;
int recordMovement(){
  //Declare a vector to store velocities during recording
  std::vector<float> outputVector = {};

  float totalTimeDelay = 0; //The total delay to run each function (minus sleeping the thread), allows for very accurate playback
  int loops = 0; //Used in order to calculate the average timeDelay

  //Record until button pressed
  while (recording){
    //Start time of this record loop
    float startTime = Brain.Timer.time(msec);

    //Loop through each motor in the motors vector
    for (vex::motor currMotor : motors){
      //Add this motor's velocity to the end of the vector as a float
      outputVector.push_back(currMotor.velocity(rpm));
    }
    //Add the current heading to check for errors during playback
    outputVector.push_back(inertialSensor.heading(vex::degrees));

    //Add to the total time delay
    totalTimeDelay += Brain.Timer.time(msec) - startTime;
    //Increment loops
    loops++;

    //Pause for a time delay to allow for playback and no crashes
    this_thread::sleep_for(recordingTimeDelay);
  }

  //Add the average and root time delay to the front of the vector for playback
  outputVector.insert(outputVector.begin(), totalTimeDelay / loops);
  outputVector.insert(outputVector.begin(), recordingTimeDelay);

  //Write the recorded velocities to the binary file in their raw signed float data to allow for faster reading
  //(See Write To File Function to look at details)
  writeToFile("output.dat", outputVector);

  //Give information about the number of bytes written to the file
  Brain.Screen.setCursor(5, 1);
  Brain.Screen.print("Size written: ");
  Brain.Screen.print(outputVector.size() * sizeof(float));

  //Return something to allow for threading
  return 0;
}

/*---------------------------------------------------------------------------*/
/*                     Follow Recorded Movement Function                     */
/*                                                                           */
/*   Follow the written motor velocities using an iterator through a vector  */
/*---------------------------------------------------------------------------*/
void followRecordedMovement(){
  //Read in the raw float data from the binary file
  //(See Read File Function to look at details)
  readFile("output.dat", &recordedMovementVector);

  //total time delay and loop for average time delay calculation which allows for accurate playback
  float totalTimeDelay = 0;
  int loops = 0;
  //Instantiate the previous desired heading variable to help with autonomous twicthing
  float previousDesiredHeading = 0;
  //Instantiate a iterator for the recordedMovementVector vector in order to retrieve recorded data
  //See the Read File and Pre-Auton Functions to see how recordedMovementVector was set up
  vector<float>::iterator iter = recordedMovementVector.begin();
  //Get the iterator data as the root time delay for accurate recording
  float rootTimeDelay = *iter;
  //Iterate forward
  iter++;
  //Get the iterator data as the average time delay for accurate recording
  float avgTimeDelay = *iter;
  //Iterate forward
  iter++;
  //While the iterator hasn't reached the end of recordedMovementVector
  while (iter < recordedMovementVector.end()){
    float startTime = Brain.Timer.time(msec);

    //Loop through each motor in the vector
    for (vex::motor currMotor : motors){
      //Set the velocity of this motor to the iterator's data
      currMotor.setVelocity(*iter, rpm);
      //Stop the motor if it should have 0 velocity
      //Helps to hold the catapult in place
      if (*iter == 0){
        currMotor.stop();
      }
      //Iterate forward
      iter++;
      //Spin the motor
      currMotor.spin(vex::forward);
    }

    //Take in the desired heading data from the iterator
    float desiredHeading = *iter;
    //Iterate
    iter++;

    if (PIDActive){
      //Calculate the heading error based on desired and actual
      float headingError = fabs(desiredHeading - inertialSensor.heading(vex::degrees));
      //Wrap around the value
      if (headingError > 180){
        headingError = 360 - headingError;
      }

      //Calculate the change in desired heading in the file in order the prevent jitters at higher rates of heading changes (During aggressive turns)
      float changeInDesired = fabs(desiredHeading - previousDesiredHeading);
      //Wrap around value if needed
      if (changeInDesired > 180){
        changeInDesired = 360 - changeInDesired;
      }

      //Check if the robot needs to be corrected during the autonomous playback
      //If the error is great enough and the changing heading isn't too high
      if (headingError > allowedHeadingError && changeInDesired < disallowHeadingErrorWhenOffset){
        float correctionStartTime = Brain.Timer.time(msec);;
        //Call the correctError method with the desired heading
        correctTurningError(desiredHeading);
        //Don't include heading error fixes in the average/total time delay
        totalTimeDelay -= Brain.Timer.time(msec) - correctionStartTime;
      }
      //Setup previousDesiredHeading for next loop
      previousDesiredHeading = desiredHeading;
    }

    //Update the total time delay and number of loops in order to keep a track of the average time delay
    totalTimeDelay += Brain.Timer.time(msec) - startTime;
    loops++;

    //Pause for a time delay to allow for playback and no crashes
    //Time delay is equal to recording time - average following time, plus 20
    //This allows for differences in reading and writing to the vector
    wait((avgTimeDelay - (totalTimeDelay / loops)) + rootTimeDelay, msec);
  }

  //Stop all motors when finished
  for (vex::motor currMotor : motors){
      currMotor.stop();
  }
}
1 Like
9

Her is the rest:

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control the robot during the autonomous phase of    */
/*  a VEX Competition.                                                       */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
  //Follows the motor velocities that were recorded to a binary file
  //See Pre-Auton Function, Read and Write To File Functions, and Follow Movement Function for more details
  //settingsMenu();
  followRecordedMovement();
}

/*---------------------------------------------------------------------------*/
/*                          Toggle Recording Function                        */
/*                                                                           */
/*                       Records the movement of the bot                     */
/*     and saves it to a binary file on the sd card for playback later       */
/*---------------------------------------------------------------------------*/
void toggleRecording(){
  //If not already recording then start recording
  if (!recording){
    //Toggle recording
    recording = true;
    //Start a thread that records the motor's velocities for playback
    //See Read, Write, Pre-Auton, and Follow Movement Functions for more details
    thread myThread = thread(recordMovement);
    Controller.Screen.setCursor(2, 1);
    Controller.Screen.print("Recording...");
  }else{
    //Toggle recording
    recording = false;
    Controller.Screen.setCursor(2, 1);
    Controller.Screen.print("Saved to slot ");
    Controller.Screen.print(slotIndex + 1);
  }
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*---------------------------------------------------------------------------*/
void usercontrol(void) {
  //Reset the timer to allow the correct time to be printed to the screen
  Brain.resetTimer();

  //Subscribe to the ButtonB.pressed callback in order to toggle recording everytime B is pressed
  Controller.ButtonB.pressed(toggleRecording);
  //Subscribe to the ButtonX.pressed callback in order to toggle the sttings menu everytime X is pressed
  Controller.ButtonX.pressed(toggleSettingsMenu);
  //Run until the code is terminated
  while (1) {
    //Drivetrain code to allow for use of mecanum wheels
    //All the negative signs show that the wheel should turn in reverse when that axis is positive
    //(Front_Right goes backwards when Axis 4 is positive / pushed to the right)
    Front_Left.setVelocity(Controller.Axis2.position(percent) + Controller.Axis4.position(percent) + Controller.Axis1.position(percent), percent);
    Front_Right.setVelocity(Controller.Axis2.position(percent) - Controller.Axis4.position(percent) - Controller.Axis1.position(percent), percent);
    Back_Left.setVelocity(Controller.Axis2.position(percent) + Controller.Axis4.position(percent) - Controller.Axis1.position(percent), percent);
    Back_Right.setVelocity(Controller.Axis2.position(percent) - Controller.Axis4.position(percent) + Controller.Axis1.position(percent), percent);
    Middle_Left.setVelocity(Controller.Axis2.position(percent) + Controller.Axis4.position(percent), percent);
    Middle_Right.setVelocity(Controller.Axis2.position(percent) - Controller.Axis4.position(percent), percent);


    //Spin each motor
    Front_Left.spin(vex::forward);
    Front_Right.spin(vex::forward);
    Back_Left.spin(vex::forward);
    Back_Right.spin(vex::forward);
    Middle_Left.spin(vex::forward);
    Middle_Right.spin(vex::forward);

    //If the catapult button is being pressed or it is in the middle of firing continue spinning the motors
    if (Controller.ButtonR2.pressing() || fmod(Catapult_Left.position(degrees), 360) < -20 || fmod(Catapult_Left.position(degrees), 360) > 20)
    {
      //Spin both motors with the right velocities
      Catapult_Left.setVelocity(catapultSpeed, rpm);
      Catapult_Right.setVelocity(catapultSpeed, rpm);
      Catapult_Left.spin(vex::forward);
      Catapult_Right.spin(vex::forward);
    }else{
      //Stop the catapults in their current position otherwise
      Catapult_Left.setVelocity(0, rpm);
      Catapult_Right.setVelocity(0, rpm);
      Catapult_Left.stop();
      Catapult_Right.stop();
    }

    // Sleep the task for a short amount of time to prevent wasted resources
    wait(movementTimeDelay, msec);
  }
}

// Main sets up the competition functions and callbacks.
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}
10

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.