amoogus sus

main.cpp

/*
Copyright 2022 Jack Meng

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <iterator>
#include <unistd.h>
#include <vector>
#include <functional>
#include <assert.h>
#include "vex.h"
#define bprint Brain.Screen.print
#define bcursor Brain.Screen.setCursor
#define sdsave Brain.SDcard.savefile
#define sdload Brain.SDcard.loadfile
#define check() Brain.SDcard.isInserted()
#define len(arr) *(&arr + 1) - arr
#define F0R(arg) for(int i = 0; i < len(arg); i++)
using namespace vex;using namespace std;

// CONFIGURATION BEGIN

const long PLAYBACK_DELAY = 23; 
const long RECORD_DELAY = 20;
const char * DELIMITER = "$%^*";
const uint8_t SYNC_TIME = 20;
const char* FILE_NAME = "FJDKSJFKLDSJFDFF.txt";
const uint8_t MAX_ELEMENT_PER_LINE = 10;
const brakeType DELIMITER_BRAKE = brakeType::coast;
#define frontLeft motor_list[0]
#define frontRight motor_list[1]
#define backLeft motor_list[2]
#define backRight motor_list[3]
#define middleRight motor_list[4]
#define middleLeft motor_list[5]
controller Controller1 = controller(primary);
brain Brain;
/*
backright == 1
backleft == 2
frontright == 3
frontleft == 4
middleright == 5
middleleft == 6
*/

motor motor_list[] = {
  motor(PORT4, true),
  motor(PORT9),
  motor(PORT7, true),
  motor(PORT15),
  motor(PORT8, true),
  motor(PORT11)
  /*
  motor(PORT4, true),
  motor(PORT9),
  motor(PORT7, true),
  motor(PORT15),
  motor(PORT8, true),
  motor(PORT11)
  */
};
motor conveyer = motor(PORT20);
motor frontLiftMotor = motor(PORT16, true);
pneumatics pneumatics0 = pneumatics(Brain.ThreeWirePort.A);
pneumatics frontLiftClamp = pneumatics(Brain.ThreeWirePort.B);
pneumatics weirdArm = pneumatics(Brain.ThreeWirePort.H);
bool togglaA = false, toggleLeft = false;


// CONFIGURATION END

// DO NOT TOUCH THE BELOW

template <typename BRUH >string tostring(BRUH t) {ostringstream ss;ss << t;return ss.str();}template <typename GCC_VECTOR >
void vtostring(GCC_VECTOR g) {for(auto &x : g) {cout << x << endl;}}template <typename SOME_INT >int toint(SOME_INT t) {stringstream ss;ss << t;int temp = NULL;ss >> temp;return temp;}
string construct_string() {return tostring(backRight.velocity(pct)) + " " + tostring(backLeft.velocity(pct)) + " " + tostring(frontRight.velocity(pct)) + " " + tostring(frontLeft.velocity(pct)) + " " + tostring(middleRight.velocity(pct)) + " " + tostring(middleLeft.velocity(pct)) + " " + tostring(conveyer.velocity(pct)) + " " + tostring(frontLiftMotor.velocity(pct)) + " " + tostring(pneumatics0.value()) + " " + tostring(frontLiftClamp.value()) + " " + tostring(weirdArm.value()) + "\n";}
string randName() {string str = "";for(int i = 0; i < 32; i++) {str[i] = (rand() % 26) + 'a';}return str;}void sprint(string content, string name = "vex_" + randName(), bool confirm = true) {uint32_t s[content.length()];if(!check()) {__throw_system_error(2);} else {for(int i = 0; i < content.length(); i++) {s[i] = (char) content[i];  }sdsave(name.c_str(), (uint8_t*) s, *(&s + 1) - s);if(confirm) {cout << "File saved as: " + name << endl;bprint(name.c_str());}}}void xprint(string content, string name = "vex" + randName(), const bool confirm = true) {ofstream fout(name);if(!check()) {__throw_system_error(2);} else {fout << content;fout.flush();if(confirm) {cout << "File saved as: " + name << endl;bprint(name.c_str());}}}
void aprint(string content, string name, const bool confirm = true) {ofstream fout(name, ios_base::app);if(!check()) {__throw_system_error(2);} else {fout << content;fout.flush();if(confirm) {cout << "File appended to: " + name << endl;bprint(name.c_str());}}}string xread(string file_name, bool confirm = true) {ifstream fin(file_name);string buffer = "";if(!check()) {__throw_system_error(2);} else {unsigned int curr = 0;string temp_line_buffer = "";while(getline(fin, temp_line_buffer)) {buffer += temp_line_buffer + DELIMITER;if(confirm) {cout << "[ Line " << curr << " ] " << temp_line_buffer.length() << " chars" << endl;}temp_line_buffer = "";curr++;}if(confirm) {cout << "[ Total read ] " << buffer << endl;}}return buffer == "" ? "NOTHING" : buffer;}
vector<string> vread(string file_name, bool confirm = true) {ifstream fin(file_name);vector<string> vs;if(!check()) {__throw_system_error(2);} else {unsigned int curr = 0;string temp_line_buffer = "";while(getline(fin, temp_line_buffer)) {vs.push_back(temp_line_buffer);if(confirm) {cout << "[ Line " << curr << " ] " << temp_line_buffer << " " << endl;}temp_line_buffer = "";curr++;}if(confirm) {cout << "[ Total read ] " << vs.size() << endl;}}return vs;}vector<string> split(string x, string delimiter) {  vector<string> v;string temp = "";for(int i = 0; i < x.length(); i++) {if(x[i] == delimiter[0]) {v.push_back(temp);temp = "";} else {temp += x[i];}}v.push_back(temp);return v;}void vtostring(vector<string> s) {for(auto &x : s) {cout << x << endl;}}
vector<int> vtoi(vector<string> s) {vector<int> st;for(string x : s) {st.push_back(toint(x));}return st;}string rread(string file_name, bool confirm = true) {ifstream fin(file_name);return nullptr;}
#define SYNC_SPEED() task::sleep(SYNC_TIME)
#define ei else if
thread t1,t2;
bool fix = false;bool w_arm = false;void kill() {t1.interrupt();t2.interrupt();t1=t2=thread();}
void flift(double d) {frontLiftMotor.spinTo(d, deg, 100, velocityUnits::pct, true);}
void togglePneu() {togglaA = Controller1.ButtonA.pressing() && togglaA ? false : true;if(togglaA){frontLiftClamp.close();}else{frontLiftClamp.open();}}void toggleBack() {toggleLeft = Controller1.ButtonLeft.pressing() && toggleLeft ? false : true;if(toggleLeft){pneumatics0.close();}else{pneumatics0.open();}}void conv1() {while(!Controller1.ButtonL1.pressing()){conveyer.spin(fwd,100,pct);}return;}void conv2() {while(!Controller1.ButtonL2.pressing()){conveyer.spin(directionType::rev,100,pct);}return;}void flip_child(){conveyer.spinFor(fwd, 100, timeUnits::msec, 100, velocityUnits::pct);}void flip_parent() {thread r(flip_child);flift(-200);flift(0);}void weird_arm() {w_arm=!w_arm?true:false;if(w_arm){weirdArm.open();}else{weirdArm.close();}}void fixer() {fix = fix ? false : true;}


// END DO NOT TOUCH

/**
USER DEFINED PLAYBACK AND RECORDING UTIL BEGIN

CHANGE THESE ONLY!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! NOT THE ABOVE!!!!
*/

void userControl() {
  Controller1.ButtonDown.pressed(kill);
  Controller1.ButtonA.pressed(togglePneu);
  Controller1.ButtonLeft.pressed(toggleBack);
  Controller1.ButtonL1.pressed(conv1);
  Controller1.ButtonL2.pressed(conv2);
  Controller1.ButtonRight.pressed(flip_parent);
  Controller1.ButtonX.pressed(weird_arm);
  Controller1.Screen.print("Press ButtonDown to stop");
  while(true) {
    frontLeft.spin(fwd, Controller1.Axis3.position()+Controller1.Axis1.position(),velocityUnits::pct);
    middleLeft.spin(fwd, Controller1.Axis3.position()+Controller1.Axis1.position(),velocityUnits::pct);
    backLeft.spin(fwd, Controller1.Axis3.position()+Controller1.Axis1.position(), velocityUnits::pct);
    frontRight.spin(fwd, Controller1.Axis3.position()-Controller1.Axis1.position(),velocityUnits::pct);
    middleRight.spin(fwd, Controller1.Axis3.position()-Controller1.Axis1.position(),velocityUnits::pct);
    backRight.spin(fwd, Controller1.Axis3.position()-Controller1.Axis1.position(),velocityUnits::pct);
    if(Controller1.ButtonDown.pressing()){kill();}
    if(Controller1.ButtonL1.pressing()){conveyer.spin(reverse, 100, pct);}ei(Controller1.ButtonL2.pressing()){conveyer.spin(fwd, 100, pct);}else{conveyer.stop(DELIMITER_BRAKE);}
    if(Controller1.ButtonRight.pressing())flip_parent();
    if(Controller1.ButtonR1.pressing()){if(fix){if(frontLiftMotor.rotation(deg)>=-1600){frontLiftMotor.spin(reverse, 100, pct);}else{frontLiftMotor.stop(hold);}}else{frontLiftMotor.spin(reverse, 100, pct);}}ei(Controller1.ButtonR2.pressing()){if(fix){if((frontLiftMotor.rotation(deg))<=-10){frontLiftMotor.spin(fwd,100,pct);}else{frontLiftMotor.stop(hold);}}else{frontLiftMotor.spin(fwd, 100, pct);}}else{frontLiftMotor.stop(hold);}
  }
}

void recording_util() {
  while(true) {
    aprint(construct_string(), FILE_NAME);
    SYNC_SPEED();
  }
}

/* br bl fr fl mr ml cr fr bc fc*/
void playback() {
  vector<string> toRead = vread(FILE_NAME, false);
  for(string x : toRead) {
    vector<int> temp = vtoi(split(x, " "));
    motor_list[0].spin(fwd, temp[0], pct);
    motor_list[1].spin(fwd, temp[1], pct);
    motor_list[2].spin(fwd, temp[2], pct);
    motor_list[3].spin(fwd, temp[3], pct);
    motor_list[4].spin(fwd, temp[4], pct);
    motor_list[5].spin(fwd, temp[5], pct);
    conveyer.spin(fwd, temp[6], pct);
    frontLiftMotor.spin(fwd, temp[7], pct);
    pneumatics0.set(temp[8]);
    frontLiftClamp.set(temp[9]);
    weirdArm.set(temp[10]);
    task::sleep(PLAYBACK_DELAY);
  }
}

void splice_playback() {
  vector<string> toRead = vread(FILE_NAME, false);
  for(string x : toRead) {
    vector<int> temp = vtoi(split(x, " "));
  }
}

/**
USER DEFINED PLAYBACK AND RECORDING END
*/

void record(void (*usercontrol)(), void(*recordingstyle)()) {
  t1 = thread(usercontrol);
  t2 = thread(recordingstyle);
  Controller1.rumble(". .");
}


int main() {
  vexcodeInit();
  //record(userControl, recording_util);
  playback();
}