Routing and HDrive fixes

Author: brsahagian

include/WPID/Chassis/Chassis.h

@@ -42,11 +42,6 @@ class Chassis {
         float straight_offset = 0;
         float turn_offset = 0;
 
-        /**
-        * Maximum acceleration for ramp
-        */
-        float max_acceleration = 1;
-        
         /**
         * The units used for distances
         */
@@ -133,13 +128,6 @@ class Chassis {
          */
         virtual void setBrakeType(vex::brakeType type) = 0;
 
-        /**
-         * @brief Set the max acceleration of the chassis. 
-         * The value is arbitrary, between 0 and 1.
-         * @param max_accel an arbitrary value to increment to ramp the speed up
-         */
-        virtual void setMaxAcceleration(float max_accel) = 0;
-
         /**
          * @brief Set the measurement units for chassis values.
          * @param preferred_units the user's measurement system

include/WPID/Chassis/HDrive.h

@@ -209,7 +209,7 @@ class HDrive : public wpid::Tank {
          * Increase the value if the robot is ramping too slow, or decrease if it causes too much jerk.
          * @param max_accel a value to increment to ramp the speed up in velocityUnits::pct
          */
-        void setMaxAcceleration(float max_accel);
+        void setMaxAcceleration(float straight_max_accel, float c_max_accel);
 
         /**
          * @brief Set the offset for the straight and turn functions.

include/WPID/Chassis/Tank.h

@@ -130,7 +130,7 @@ class Tank : public wpid::Chassis{
          * Increase the value if the robot is ramping too slow, or decrease if it causes too much jerk.
          * @param max_accel a value to increment to ramp the speed up in velocityUnits::pct
          */
-        void setMaxAcceleration(float max_accel) override;
+        void setMaxAcceleration(float max_accel);
 
         /**
          * @brief Set the offset for the straight and turn functions.

include/init.h

@@ -10,7 +10,7 @@ using namespace vex;
 extern brain Brain;
 extern controller* con;
 
-extern Tank* chassis;
+extern HDrive* chassis;
 extern Mechanism* fourbar;
 
 void init(void);

src/WPID/Chassis/HDrive.cpp

@@ -34,6 +34,7 @@ void HDrive::setTurnPID(PID pid){
 
 void HDrive::setStrafePID(PID pid){
     pidStrafe = pid;
+    center->setPID(pidStrafe);
 }
 
 void HDrive::spin(int left_velocity, int right_velocity, int center_velocity){
@@ -53,9 +54,14 @@ void HDrive::straight(float distance, int max_speed){
 
 void HDrive::straightAsync(float distance, int max_speed){
     distance = Conversion::standardize(distance, this->measure_units);
-    float target = ((distance + straight_offset) / wheel_circumference) * 360.0;
-    left->setPID(pidStraight);
-    right->setPID(pidStraight);
+    if(distance > 0){
+        distance += straight_offset;
+    } else {
+        distance -= straight_offset;
+    }
+    float target = ((distance) / wheel_circumference) * 360.0;
+    left->setPID(pidStraight.copy());
+    right->setPID(pidStraight.copy());
     this->spinToTarget(target, target, 0, max_speed, max_speed, 0);
 }
 
@@ -65,9 +71,14 @@ void HDrive::turn(int target_angle, int max_speed){
 }
 
 void HDrive::turnAsync(float target_angle, int max_speed){
-    float target = ((track_width/2)*((float)(target_angle+turn_offset)*M_PI/180)/wheel_circumference)*360;
-    left->setPID(pidTurn);
-    right->setPID(pidTurn);
+    if(target_angle > 0){
+        target_angle += turn_offset;
+    } else {
+        target_angle -= turn_offset;
+    }
+    float target = ((track_width/2)*((float)(target_angle)*M_PI/180)/wheel_circumference)*360;
+    left->setPID(pidTurn.copy());
+    right->setPID(pidTurn.copy());
     this->spinToTarget(target, -target, 0, max_speed, max_speed, 0);
 }
 
@@ -78,7 +89,12 @@ void HDrive::strafe(float distance, int max_speed){
 
 void HDrive::strafeAsync(float distance, int max_speed){
     distance = Conversion::standardize(distance, this->measure_units);
-    float target = ((distance + strafe_offset) / center_wheel_circumference) * 360.0;
+     if(distance > 0){
+        distance += strafe_offset;
+    } else {
+        distance -= strafe_offset;
+    }
+    float target = ((distance) / center_wheel_circumference) * 360.0;
     this->spinToTarget(0, 0, target, 0, 0, max_speed);
 }
 
@@ -99,9 +115,9 @@ void HDrive::diagonalAsync(float straight_distance, float strafe_distance, int s
 }
 
 void HDrive::spinToTarget(float left_target, float right_target, float center_target, int l_max_spd, int r_max_spd, int c_max_spd){
-    left->moveRelative(left_target, l_max_spd);
-    right->moveRelative(right_target, r_max_spd);
-    center->moveRelative(center_target, c_max_spd);
+    left->moveRelativeAsync(left_target, l_max_spd);
+    right->moveRelativeAsync(right_target, r_max_spd);
+    center->moveRelativeAsync(center_target, c_max_spd);
 }
 
 void HDrive::stop(){
@@ -146,8 +162,12 @@ void HDrive::setOffset(float straight, float turn, float center){
     strafe_offset = center;
 }
 
-void HDrive::setMaxAcceleration(float max_accel){
-    this->max_acceleration = max_accel;
+void HDrive::setMaxAcceleration(float straight_max_accel, float c_max_accel){
+    if(straight_max_accel < 0 || c_max_accel < 0)
+        LOG(WARN) << "Negative accelerations not allowed";
+    this->left->setMaxAcceleration(straight_max_accel);
+    this->right->setMaxAcceleration(straight_max_accel);
+    this->center->setMaxAcceleration(c_max_accel);
 }
 
 void HDrive::setTimeout(int timeout){

src/WPID/Chassis/Tank.cpp

@@ -111,7 +111,6 @@ void Tank::setOffset(float straight, float turn){
 void Tank::setMaxAcceleration(float max_accel){
     if(max_accel < 0)
         LOG(WARN) << "Negative accelerations not allowed";
-    this->max_acceleration = max_accel;
     this->left->setMaxAcceleration(max_accel);
     this->right->setMaxAcceleration(max_accel);
 }

src/auton.cpp

@@ -5,19 +5,21 @@ int printAngle(){
     while(1){
         Brain.Screen.clearScreen();
         Brain.Screen.setCursor(1, 1);
-        Brain.Screen.print("Angle: %f", fourbar->getPosition(deg));
+        //Brain.Screen.print("Angle: %f", fourbar->getPosition(deg));
         this_thread::sleep_for(25);
     }
     return 0;
 }
 
 void auton(){
-  thread printAngleThread = thread(printAngle);
-  // chassis->straight(24, 75);
-  // chassis->straight(-24, 75);
-  // chassis->turn(90, 25);
-  // chassis->turn(-90, 25);
-  fourbar->moveAbsolute(60, 75);
-  fourbar->moveAbsolute(90, 75);
-  fourbar->moveAbsolute(0, 75);
+  //thread printAngleThread = thread(printAngle);
+  // fourbar->moveAbsolute(60, 70);
+  // fourbar->moveAbsolute(110, 60);
+  // fourbar->moveAbsolute(0, 70);
+
+  chassis->strafe(24,40);
+  chassis->turn(90,35);
+  chassis->diagonal(-24, -24, 40);
+  chassis->turn(-90,35);
+  chassis->straight(-24,40);
 }

src/init.cpp

@@ -3,27 +3,29 @@
 brain Brain;
 controller* con = new controller(controllerType::primary);
 
-Tank* chassis;
-Mechanism* fourbar;
+HDrive* chassis;
+//Mechanism* fourbar;
 
 motor leftFront = motor(PORT17, ratio18_1, false);
 motor leftBack = motor(PORT18, ratio18_1, false);
+motor center = motor(PORT16, ratio18_1, true);
 
 motor rightFront = motor(PORT19, ratio18_1, true);
 motor rightBack = motor(PORT20, ratio18_1, true);
 
 motor_group leftGroup = motor_group(leftFront, leftBack);
 motor_group rightGroup = motor_group(rightFront, rightBack);
+motor_group centerGroup = motor_group(center);
 
-motor mech = motor(PORT11, ratio18_1, false);
-motor_group mechGroup = motor_group(mech);
+//motor mech = motor(PORT11, ratio18_1, false);
+//motor_group mechGroup = motor_group(mech);
 
 void init(void) {
     Brain.Screen.clearScreen();
-    chassis = new Tank(12.5, 1.625, &leftGroup, &rightGroup, 1);
-    chassis->setOffset(0, 5);
+    chassis = new HDrive(12.5, 1.625, 1.625, &leftGroup, &rightGroup, &centerGroup, 1);
+    chassis->setOffset(0, 3, 0);
     chassis->setBrakeType(brakeType::brake);
-    chassis->setMaxAcceleration(2);
+    chassis->setMaxAcceleration(2, 2);
 
     PID straight = PID(0.2, 0.65, 0.02);
     straight.setBias(0);
@@ -32,7 +34,7 @@ void init(void) {
     straight.setDelayTime(20);
     straight.setErrorRange(2);
 
-    PID turn = PID(0.15, 0.65, 0.02);
+    PID turn = PID(0.15, 0.7, 0.01);
     turn.setBias(0);
     turn.setMaxIntegral(8);
     turn.setLowSpeedThreshold(3);
@@ -41,15 +43,25 @@ void init(void) {
 
     chassis->setStraightPID(straight);
     chassis->setTurnPID(turn);
+    chassis->setMeasurementUnits(Conversion::measurement::in);
+
+    PID strafe = PID(0.3, 0.67, 0.02);
+    strafe.setBias(0);
+    strafe.setMaxIntegral(10);
+    strafe.setLowSpeedThreshold(5);
+    strafe.setDelayTime(20);
+    strafe.setErrorRange(2);
+
+    chassis->setStrafePID(strafe);
 
     // Fourbar setup
-    fourbar = new Mechanism(&mechGroup, 0.25);
-    fourbar->setBrakeType(brakeType::hold);
-    fourbar->setMaxAcceleration(5);
-    fourbar->setBounds(0, 110);
+    // fourbar = new Mechanism(&mechGroup, 0.25);
+    // fourbar->setBrakeType(brakeType::hold);
+    // fourbar->setMaxAcceleration(5);
+    // fourbar->setBounds(0, 110);
 
-    PID lift = PID(1.3, 0.7, 0.02);
-    fourbar->setPID(lift);
+    // PID lift = PID(1.5, 0.7, 0.02);
+    // fourbar->setPID(lift);
 
     LOG().setBaseLevel(DEBUG);
     LOG(INFO) << "Robot Initialized";

src/usercontrol.cpp

@@ -7,27 +7,40 @@ double scale(double x) {
     return (x > 0 ? 100 : -100) * ((x/127) * (x/127)); 
 }
 
+int printInDriverControl(){
+    while(1){
+        Brain.Screen.clearScreen();
+        Brain.Screen.setCursor(30, 30);
+        Brain.Screen.print("In DRIVER CONTROL mode");
+        this_thread::sleep_for(25);
+    }
+    return 0;
+}
+
 void usercontrol(){ 
     auton();
-    Brain.Screen.printAt(1, 1, "IN DRIVER CONTROL");
+    thread printAngleThread = thread(printInDriverControl);
+    //Brain.Screen.printAt(1, 1, "IN DRIVER CONTROL");
     // axis values on controller
-    double axis3, axis4 = 0;
+    double axis1, axis3, axis4= 0;
     while (1) {
         axis3 = scale(con->Axis3.value());
         axis4 = scale(con->Axis4.value());
+        axis1 = scale(con->Axis1.value());
+    
 
-        if(con->ButtonR1.pressing()){
-            fourbar->spin(20);
-        } else if (con->ButtonR2.pressing()) {
-            fourbar->spin(-20);
-        } else {
-            fourbar->stop();
-        }
+        // if(con->ButtonR1.pressing()){
+        //     fourbar->spin(20);
+        // } else if (con->ButtonR2.pressing()) {
+        //     fourbar->spin(-20);
+        // } else {
+        //     fourbar->stop();
+        // }
 
         if(con->ButtonDown.pressing()){
-            chassis->spin(-20);
+            chassis->spin(-20,0);
         } else {
-            chassis->spin(axis3 + axis4, axis3 - axis4);
+            chassis->spin(axis3 + axis4, axis3 - axis4, axis1);
         }
 
         wait(20, msec);