diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp index c30d9a3..d8322cf 100644 --- a/lib/Chassis/src/chassis.cpp +++ b/lib/Chassis/src/chassis.cpp @@ -108,9 +108,6 @@ bool Chassis::ChassisLoop(Twist& velocity) Serial.print('\n'); #endif - // motor updates - UpdateMotors(); - /* Update the wheel velocity so it gets back to Robot. */ velocity = CalcOdomFromWheelMotion(); @@ -139,30 +136,6 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right) rightMotor.SetMotorEffortDirect(right); } -void Chassis::UpdateMotors(void) -{ - leftMotor.ControlMotorSpeed(); - rightMotor.ControlMotorSpeed(); -} - -/** - * SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds. - */ -void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec) -{ - /** - * TODO: Check the code below. You did this in Lab 1, so we give you the calcs. - */ - leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.); - rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.); -} - -void Chassis::SetTwist(const Twist& twist) -{ - /** - * TODO: Complete SetTwist() to call SetWheelSpeeds() from target u and omega - */ -} Twist Chassis::CalcOdomFromWheelMotion(void) { diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h index c6ecd50..f08f44e 100644 --- a/lib/Chassis/src/chassis.h +++ b/lib/Chassis/src/chassis.h @@ -42,12 +42,10 @@ public: void SetTwist(const Twist& twist); Twist CalcOdomFromWheelMotion(void); - /** - * A utility function for converting robot speed to wheel speed. Left - * public so that you can test more easily. - */ - void SetWheelSpeeds(float, float); - void Stop(void) { SetWheelSpeeds(0, 0); } + + void SetMotorEfforts(int16_t, int16_t); + + void Stop(void) { SetMotorEfforts(0, 0);} protected: /** @@ -57,5 +55,4 @@ protected: void InitializeMotors(void); void UpdateMotors(void); - void SetMotorEfforts(int16_t, int16_t); }; diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorParameters.h b/lib/Romi32U4Motors/src/Romi32U4MotorParameters.h deleted file mode 100644 index 76f2edd..0000000 --- a/lib/Romi32U4Motors/src/Romi32U4MotorParameters.h +++ /dev/null @@ -1,5 +0,0 @@ -#define KP_MOTOR 5.0 -#define KI_MOTOR 0.5 -#define KD_MOTOR 0.0 // don't use - -#define INTEGRAL_CAP_MOTOR 0.0 // if you want to add an integral cap \ No newline at end of file diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h index a7777c7..22d9a1c 100644 --- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h @@ -2,7 +2,6 @@ #include #include -#include "Romi32U4MotorParameters.h" // define the motor pins here #define PWM_L 10 @@ -24,30 +23,12 @@ protected: // For prettier printing const String name; - // Used to control the motors in different ways - enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED}; - volatile CTRL_MODE ctrlMode = CTRL_DIRECT; - - // Used to manage PID coefficients; note that the defaults stink for speed control. - float Kp = KP_MOTOR; - float Ki = KI_MOTOR; - float Kd = KD_MOTOR; - - // Used to keep track of the target speed, in counts / interval. - float targetSpeed = 0; - /** * This is the speed of the motor, in "encoder counts / encoder interval". * The encoder interval is set in Robot::InitializeMotorControlTimer. */ volatile int16_t speed = 0; - // For tracking the error integral (sum) - float sumError = 0; - - // For tracking derivative (difference) - float prevError = 0; - // Maximum effort int16_t maxEffort = 420; @@ -69,7 +50,6 @@ protected: */ void SetMotorEffortDirect(int16_t effort) { - ctrlMode = CTRL_DIRECT; SetEffort(effort); } @@ -93,63 +73,19 @@ protected: } /** - * Sets the target speed in "encoder ticks/16 ms interval" - * */ - void SetTargetSpeed(float target) + * GetEncoderTotal() returns the total encoder counts" + * + */ + int16_t GetEncoderTotal(void) { - targetSpeed = target; + cli(); + int16_t currCount = encCount; + sei(); - if(ctrlMode != CTRL_SPEED) - { - // Reset the error integral if we are switching from another mode - // Otherwise, the robot may jump due to residual integral - sumError = 0; - - // Also set prevCount to encCount so to avoid speed jumps when switching mode - CalcEncoderDelta(); - } - - ctrlMode = CTRL_SPEED; + return currCount; } - /** - * ControlMotorSpeed implements the PID controller. It should _not_ be called by user code. - * Instead, ControlMotorSpeed is called from Chassis::UpdateMotors, which is on a timer schedule. - */ - void ControlMotorSpeed(void) - { - if(ctrlMode == CTRL_SPEED) - { - /** - * TODO: Implement PI controller. Only P at the moment. - */ - // Calculate the error in speed - float error = targetSpeed - speed; - - sumError+= error; - - int16_t effort = Kp * error + Ki * sumError; - - // Set the effort for the motor - SetEffort(effort); - -#ifdef __MOTOR_DEBUG__ - Serial.print('>'); - Serial.print(name); - Serial.print("_target:"); - Serial.println(targetSpeed); - Serial.print('>'); - Serial.print(name); - Serial.print("_speed:"); - Serial.println(speed); - Serial.print('>'); - Serial.print(name); - Serial.print("_error:"); - Serial.println(error); -#endif - } - } static void AttachInterrupts(void); @@ -193,7 +129,6 @@ protected: public: Romi32U4MotorBase(const String& nm) : name(nm) {} - void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;} }; template diff --git a/platformio.ini b/platformio.ini index 293d59e..65db724 100644 --- a/platformio.ini +++ b/platformio.ini @@ -17,7 +17,6 @@ monitor_speed = 115200 lib_deps = Wire - https://github.com/gcl8a/IRDecoder https://github.com/gcl8a/Button https://github.com/gcl8a/event_timer https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp index 82c1a46..8319790 100644 --- a/src/robot-nav.cpp +++ b/src/robot-nav.cpp @@ -59,7 +59,7 @@ void Robot::DriveToPoint(void) #endif /** - * TODO: Call chassis.SetTwist() to command the motion, based on your calculations above. + * TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above. */ } } diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp deleted file mode 100644 index 73bb953..0000000 --- a/src/robot-remote.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/** - * robot-remote.cpp implements features of class Robot that are related to processing - * remote control commands. It also manages modes. You might want to trim away some of - * the code that isn't needed later in the term. - */ -#include "robot.h" - -#include -#include - -/** - * IRDecoder decoder is declared extern in IRdecoder.h (for ISR purposes), - * so we _must_ name it decoder. - */ -#define IR_PIN 17 -IRDecoder decoder(IR_PIN); - -void Robot::HandleKeyCode(int16_t keyCode) -{ - Serial.println(keyCode); - - // Regardless of current state, if ENTER is pressed, go to idle state - if(keyCode == ENTER_SAVE) { EnterIdleState(); keyString = "";} - - - // If STOP/MODE is pressed, it toggles control mode (auto <-> teleop) - else if(keyCode == STOP_MODE) - { - if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); } - else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); } - EnterIdleState(); // Idle to avoid surprises - keyString = ""; - } - - if(robotCtrlMode == CTRL_AUTO) - { - /** - * TODO: Add destinations. - */ - switch(keyCode) - { - case NUM_2: - SetDestination(Pose(60, 0, 0)); - break; - case NUM_4: - break; - case NUM_6: - break; - case NUM_8: - break; - } - keyString = ""; - } - - else if(robotCtrlMode == CTRL_TELEOP) - { - switch(keyCode) - { - case UP_ARROW: - chassis.SetTwist(Twist(20, 0, 0)); - break; - case RIGHT_ARROW: - break; - case DOWN_ARROW: - break; - case LEFT_ARROW: - break; - } - keyString = ""; - } -} - -void Robot::EnterTeleopMode(void) -{ - Serial.println("-> TELEOP"); - robotCtrlMode = CTRL_TELEOP; -} - -void Robot::EnterAutoMode(void) -{ - Serial.println("-> AUTO"); - robotCtrlMode = CTRL_AUTO; -} diff --git a/src/robot.cpp b/src/robot.cpp index 03d5e61..6d1bc35 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1,16 +1,9 @@ #include "robot.h" -#include void Robot::InitializeRobot(void) { chassis.InititalizeChassis(); - /** - * Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp - * for instantiation and setting the pin. - */ - decoder.init(); - /** * TODO: Set pin 13 HIGH when navigating and LOW when destination is reached. * Need to set as OUTPUT here. @@ -27,17 +20,11 @@ void Robot::EnterIdleState(void) /** * The main loop for your robot. Process both synchronous events (motor control), - * and asynchronous events (IR presses, distance readings, etc.). + * and asynchronous events (distance readings, etc.). */ void Robot::RobotLoop(void) { - /** - * Handle any IR remote keypresses. - */ - int16_t keyCode = decoder.getKeyCode(); - if(keyCode != -1) HandleKeyCode(keyCode); - - /** + /** * Run the chassis loop, which handles low-level control. */ Twist velocity; @@ -45,7 +32,8 @@ void Robot::RobotLoop(void) { // We do FK regardless of state UpdatePose(velocity); - + chassis.SetMotorEfforts(220,-220); + /** * Here, we break with tradition and only call these functions if we're in the * DRIVE_TO_POINT state. CheckReachedDestination() is expensive, so we don't want diff --git a/src/robot.h b/src/robot.h index eee7c0a..df8a8d0 100644 --- a/src/robot.h +++ b/src/robot.h @@ -5,17 +5,6 @@ class Robot { protected: - /** - * We define some modes for you. SETUP is used for adjusting gains and so forth. Most - * of the activities will run in AUTO. You shouldn't need to mess with these. - */ - enum ROBOT_CTRL_MODE - { - CTRL_TELEOP, - CTRL_AUTO, - }; - ROBOT_CTRL_MODE robotCtrlMode = CTRL_AUTO; - /** * robotState is used to track the current task of the robot. You will add new states as * the term progresses. @@ -45,16 +34,9 @@ public: void RobotLoop(void); protected: - /* For managing IR remote key presses*/ - void HandleKeyCode(int16_t keyCode); - /* State changes */ void EnterIdleState(void); - /* Mode changes */ - void EnterTeleopMode(void); - void EnterAutoMode(void); - // /* Navigation methods.*/ void UpdatePose(const Twist& u); void SetDestination(const Pose& destination);