From 9251f1021783c42cc6d431c4cc8ba9b250d5fcce Mon Sep 17 00:00:00 2001 From: d-arcymarie Date: Thu, 3 Mar 2022 23:40:27 -0800 Subject: [PATCH 1/7] Line follow w faster motors --- Kabob_Code/src/StateMachine/StateMachine.cpp | 34 ++++---- Kabob_Code/src/System.h | 8 +- Kabob_Code/src/Test/Test.cpp | 85 ++------------------ 3 files changed, 27 insertions(+), 100 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 884734b..340bfdb 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -1,9 +1,9 @@ #include "StateMachine.h" -#define VELOCITY 255 +#define VELOCITY 100 -#define Right_TURN_TIME 4000 -#define SECOUND_TURN_TIME 2500 +#define Right_TURN_TIME 2100 +#define SECOUND_TURN_TIME 550 States_t state; States_r line_state; @@ -89,9 +89,9 @@ void handleLoadState(void) { shephard.claw.open(); } else if (timeInState > 1000 && timeInState < 2000) { shephard.claw.close(); - } else if (timeInState > 2000 && timeInState < 5000) { - shephard.chassis.move_forward_at_speed(VELOCITY); - } else if (timeInState > 5000) { + } else if (timeInState > 2000 && timeInState < 2500) { + shephard.chassis.move_forward_at_speed(150); + } else if (timeInState > 3500) { changeStateTo(STATE_NAV_TARGET); changeZoneTo(ZONE_1); } @@ -127,8 +127,8 @@ void handleNavTargetState(void){ break; case ZONE_3: // turn 90 degrees and then line follow - if (t > 1000 && t < 1000+Right_TURN_TIME) { - shephard.chassis.turn_right(200); + if (t > 300 && t < 300+Right_TURN_TIME) { + shephard.chassis.turn_right(100); } else { lineFollow(); } @@ -136,13 +136,13 @@ void handleNavTargetState(void){ case ZONE_4: // turn less than 90 degrees and then line follow if (t < SECOUND_TURN_TIME) { - shephard.chassis.turn_left(200); + shephard.chassis.turn_left(100); } else { lineFollow(); } break; case ZONE_TARGET: - if (t < 1000) { + if (t < 400) { shephard.chassis.move_forward_at_speed(VELOCITY); } else { changeStateTo(STATE_UNLOAD); @@ -178,10 +178,10 @@ void checkForZoneChange(void) { } else if (right && zone == ZONE_2) { setFlag(flagRightLine); changeZoneTo(ZONE_3); - } else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) { + } else if (left && zone == ZONE_3 && curr_time - zone_time > 2000) { changeZoneTo(ZONE_4); setFlag(flagLeftLine); - } else if (left && zone == ZONE_4 && curr_time - zone_time > 6000) { + } else if (left && zone == ZONE_4 && curr_time - zone_time > 2000) { changeZoneTo(ZONE_TARGET); } } @@ -210,19 +210,19 @@ void lineFollow(void) { switch(line_state) { case STATE_ON_LINE: - shephard.chassis.move_forward_at_speed(VELOCITY); + shephard.chassis.move_forward_at_speed(130); break; case STATE_OFF_RIGHT: - shephard.chassis.veer_forward(250, 180); + shephard.chassis.veer_forward(140, 100); break; case STATE_OFF_SLIGHT_RIGHT: - shephard.chassis.veer_forward(250, 200); + shephard.chassis.veer_forward(130, 100); break; case STATE_OFF_LEFT: - shephard.chassis.veer_forward(180, 250); + shephard.chassis.veer_forward(100, 140); break; case STATE_OFF_SLIGHT_LEFT: - shephard.chassis.veer_forward(200, 250); + shephard.chassis.veer_forward(100, 130); break; default: Serial.println("humm"); diff --git a/Kabob_Code/src/System.h b/Kabob_Code/src/System.h index 4632697..aeb3e96 100644 --- a/Kabob_Code/src/System.h +++ b/Kabob_Code/src/System.h @@ -8,13 +8,13 @@ #define RIGHT_BACKWARD_PIN 4 #define RIGHT_FORWARD_PIN 3 -#define LEFT_BACKWARD_PIN 10 -#define LEFT_FORWARD_PIN 9 +#define LEFT_BACKWARD_PIN 9 +#define LEFT_FORWARD_PIN 10 #define ENCODER_RIGHT_PIN1 8 -#define ENCODER_RIGHT_PIN2 11 +#define ENCODER_RIGHT_PIN2 1 #define ENCODER_LEFT_PIN1 7 -#define ENCODER_LEFT_PIN2 5 +#define ENCODER_LEFT_PIN2 0 #define SERVO_PIN 6 diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index aafdb27..98b59e4 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -1,85 +1,12 @@ #include "Test.h" -#define VELOCITY 255 -#define FLAG_TIME 1000 - -unsigned long state_time; -unsigned long serial_time; -unsigned long curr_time; -unsigned long zone_time; - -uint8_t servoPin = 6; -uint8_t servoPos = 0; - -uint8_t flagLeftLine = 0; -uint8_t flagRightLine = 0; -States_r line_state; -unsigned long flag_time; - - void setup(){ Serial.begin(9600); -} -void loop(void){ - lineFollow(); - checkFlags(); -} - - -void checkFlags(void) { - if (curr_time - flag_time > FLAG_TIME) { - flagLeftLine = 0; - flagRightLine = 0; - } -} - - -void lineFollow(void) { - uint8_t center_left = shephard.sensors.line.center_left.read(); - uint8_t center_middle = shephard.sensors.line.center_middle.read(); - uint8_t center_right = shephard.sensors.line.center_right.read(); - // primary line sensing states - if (center_middle && !center_right && !center_left){ - changeLineStateTo(STATE_ON_LINE); - } - else if (!center_middle && center_right && !center_left) { - changeLineStateTo(STATE_OFF_LEFT); - } - else if (!center_middle && !center_right && center_left) { - changeLineStateTo(STATE_OFF_RIGHT); - } - else if (center_middle && !center_right && center_left && !flagLeftLine) { - changeLineStateTo(STATE_OFF_SLIGHT_RIGHT); - } - else if (center_middle && center_right && !center_left && !flagRightLine) { - changeLineStateTo(STATE_OFF_SLIGHT_LEFT); - } - - switch(line_state) { - case STATE_ON_LINE: - shephard.chassis.move_forward_at_speed(VELOCITY); - break; - case STATE_OFF_RIGHT: - shephard.chassis.veer_forward(250, 180); - break; - case STATE_OFF_SLIGHT_RIGHT: - shephard.chassis.veer_forward(250, 200); - break; - case STATE_OFF_LEFT: - shephard.chassis.veer_forward(180, 250); - break; - case STATE_OFF_SLIGHT_LEFT: - shephard.chassis.veer_forward(200, 250); - break; - default: - Serial.println("humm"); - } - + Serial.println("Starting"); } - -void changeLineStateTo(States_r s) { - if (s != line_state) { - line_state = s; - } -} \ No newline at end of file +void loop(void){ + Serial.println("ah"); + shephard.chassis.move_forward_at_speed(200); + delay(1000); +} \ No newline at end of file From 79b60d5832e8077f22fd5b510c57d38f368a68ba Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Fri, 4 Mar 2022 09:15:51 -0800 Subject: [PATCH 2/7] forward, backward, cw, and ccw are working with encoders --- Kabob_Code/src/Actuators/Chassis.cpp | 20 ++++++++--------- Kabob_Code/src/Actuators/Chassis.h | 22 ++++++++++--------- Kabob_Code/src/Actuators/MotorWithEncoder.cpp | 5 +++++ Kabob_Code/src/Actuators/MotorWithEncoder.h | 2 +- Kabob_Code/src/StateMachine/StateMachine.cpp | 7 +++++- Kabob_Code/src/System.cpp | 3 ++- Kabob_Code/src/System.h | 8 +++---- Kabob_Code/src/Test/Test.cpp | 7 +++--- 8 files changed, 44 insertions(+), 30 deletions(-) diff --git a/Kabob_Code/src/Actuators/Chassis.cpp b/Kabob_Code/src/Actuators/Chassis.cpp index b2eb3a9..112cdc9 100644 --- a/Kabob_Code/src/Actuators/Chassis.cpp +++ b/Kabob_Code/src/Actuators/Chassis.cpp @@ -10,15 +10,15 @@ void Chassis::stop(void) void Chassis::move_forward(double distance, uint8_t speed) { double movement_degrees = distance / wheel_circumference * 360; - right.cw_at_speed(speed); - left.ccw_at_speed(speed); + right.move(movement_degrees, speed); + left.move(-movement_degrees, speed); } void Chassis::move_backward(double distance, uint8_t speed) { double movement_degrees = distance / wheel_circumference * 360; - right.ccw_at_speed(speed); - left.cw_at_speed(speed); + right.move(-movement_degrees, speed); + left.move(movement_degrees, speed); } void Chassis::turn_right(uint8_t speed) @@ -47,16 +47,16 @@ void Chassis::veer_backward(uint8_t speed_right, uint8_t speed_left) void Chassis::turn_cw(double angle, uint8_t speed) { - double movement_degrees = chassis_circumference / (angle / 360) / wheel_circumference * 360; - right.ccw_at_speed(speed); - left.ccw_at_speed(speed); + double movement_degrees = chassis_circumference * (angle / 360) / wheel_circumference * 360; + right.ccw(movement_degrees, speed); + left.ccw(movement_degrees, speed); } void Chassis::turn_ccw(double angle, uint8_t speed) { - double movement_degrees = chassis_circumference / (angle / 360) / wheel_circumference * 360; - right.cw_at_speed(speed); - left.cw_at_speed(speed); + double movement_degrees = chassis_circumference * (angle / 360) / wheel_circumference * 360; + right.cw(movement_degrees, speed); + left.cw(movement_degrees, speed); } void Chassis::move_forward_at_speed(uint8_t speed) diff --git a/Kabob_Code/src/Actuators/Chassis.h b/Kabob_Code/src/Actuators/Chassis.h index c3da426..76587a0 100644 --- a/Kabob_Code/src/Actuators/Chassis.h +++ b/Kabob_Code/src/Actuators/Chassis.h @@ -2,6 +2,7 @@ #include "Arduino.h" #include "Motor.h" +#include "MotorWithEncoder.h" class Chassis { @@ -16,8 +17,8 @@ class Chassis unsigned le_pin2, double hub_to_hub_distance, double wheel_circumference, - uint8_t min_speed) : right(rf_pin, rb_pin, min_speed), - left(lf_pin, lb_pin, min_speed), + uint8_t min_speed) : right(rf_pin, rb_pin, re_pin1, re_pin2, min_speed), + left(lf_pin, lb_pin, le_pin1, le_pin2, min_speed), chassis_circumference(hub_to_hub_distance * PI), wheel_circumference(wheel_circumference), ninety_degrees(chassis_circumference / 4 / wheel_circumference * 360){}; @@ -29,14 +30,14 @@ class Chassis * distance (centimeters) * - if stop() is called, the Chassis will reset its target. */ - void move_forward(double distance, uint8_t speed = 50); - void move_backward(double distance, uint8_t speed = 50); + void move_forward(double distance, uint8_t speed = 150); + void move_backward(double distance, uint8_t speed = 150); /* turn_right and turn_left * rotate 90° */ - void turn_right(uint8_t speed = 50); - void turn_left(uint8_t speed = 50); + void turn_right(uint8_t speed = 150); + void turn_left(uint8_t speed = 150); void veer_forward(uint8_t speed_right, uint8_t speed_left); void veer_backward(uint8_t speed_right, uint8_t speed_left); @@ -45,19 +46,20 @@ class Chassis * distance (centimeters) * - if stop() is called, the Chassis will reset its target. */ - void turn_cw(double angle, uint8_t speed = 50); - void turn_ccw(double angle, uint8_t speed = 50); + void turn_cw(double angle, uint8_t speed = 150); + void turn_ccw(double angle, uint8_t speed = 150); void move_forward_at_speed(uint8_t speed); void move_backward_at_speed(uint8_t speed); void turn_right_at_speed(uint8_t speed); void turn_left_at_speed(uint8_t speed); - Motor right; - Motor left; + MotorWithEncoder right; + MotorWithEncoder left; private: double chassis_circumference, wheel_circumference, ninety_degrees; + uint8_t executing; }; \ No newline at end of file diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp index cb05f73..7d598e5 100644 --- a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp +++ b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp @@ -3,6 +3,11 @@ void MotorWithEncoder::activity(){ pos = abs(encoder.read()); + + Serial.println("position = "); + Serial.println(pos); + Serial.println("target : "); + Serial.println(target); // overflow protection if (pos > 0xFFFFFF){ diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.h b/Kabob_Code/src/Actuators/MotorWithEncoder.h index dd8f537..83b7a35 100644 --- a/Kabob_Code/src/Actuators/MotorWithEncoder.h +++ b/Kabob_Code/src/Actuators/MotorWithEncoder.h @@ -6,7 +6,7 @@ #define ENCODER_OPTIMIZE_INTERRUPTS #include -class MotorWithEncoder : Motor +class MotorWithEncoder : public Motor { public: MotorWithEncoder(unsigned f_pin, diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 340bfdb..991028d 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -20,6 +20,10 @@ uint8_t servoPos = 0; uint8_t flagLeftLine = 0; uint8_t flagRightLine = 0; +//if this is true, do not allow motor to be set on different course +// unless extreme reason (i.e. hole) +uint8_t navigationSet = 0; + unsigned long flag_time; void setup() { @@ -47,6 +51,7 @@ void loop() { checkGlobalEvents(); checkFlags(); checkForZoneChange(); + shephard.chassis.activity(); // TO LINE FOLLOW: Must be in state = state_nav_target @@ -91,7 +96,7 @@ void handleLoadState(void) { shephard.claw.close(); } else if (timeInState > 2000 && timeInState < 2500) { shephard.chassis.move_forward_at_speed(150); - } else if (timeInState > 3500) { + } else if (timeInState > 2500) { changeStateTo(STATE_NAV_TARGET); changeZoneTo(ZONE_1); } diff --git a/Kabob_Code/src/System.cpp b/Kabob_Code/src/System.cpp index 8c1d017..579bbfd 100644 --- a/Kabob_Code/src/System.cpp +++ b/Kabob_Code/src/System.cpp @@ -2,7 +2,8 @@ void System::activity(void) { - // + chassis.left.activity(); + chassis.right.activity(); } System shephard; \ No newline at end of file diff --git a/Kabob_Code/src/System.h b/Kabob_Code/src/System.h index aeb3e96..c13ba6c 100644 --- a/Kabob_Code/src/System.h +++ b/Kabob_Code/src/System.h @@ -11,10 +11,10 @@ #define LEFT_BACKWARD_PIN 9 #define LEFT_FORWARD_PIN 10 -#define ENCODER_RIGHT_PIN1 8 -#define ENCODER_RIGHT_PIN2 1 -#define ENCODER_LEFT_PIN1 7 -#define ENCODER_LEFT_PIN2 0 +#define ENCODER_RIGHT_PIN1 7 +#define ENCODER_RIGHT_PIN2 8 +#define ENCODER_LEFT_PIN1 2 +#define ENCODER_LEFT_PIN2 1 #define SERVO_PIN 6 diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index 98b59e4..502c0b3 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -2,11 +2,12 @@ void setup(){ Serial.begin(9600); + while(!Serial); Serial.println("Starting"); + shephard.chassis.turn_ccw(90, 150); } void loop(void){ - Serial.println("ah"); - shephard.chassis.move_forward_at_speed(200); - delay(1000); + shephard.activity(); + delay(100); } \ No newline at end of file From 524559f5e870af33c20db075a1bc049fd48a3415 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Fri, 4 Mar 2022 09:50:19 -0800 Subject: [PATCH 3/7] not tested on course --- Kabob_Code/src/Actuators/MotorWithEncoder.cpp | 9 ++- Kabob_Code/src/Actuators/MotorWithEncoder.h | 1 + Kabob_Code/src/StateMachine/StateMachine.cpp | 66 +++++++++++-------- Kabob_Code/src/StateMachine/StateMachine.h | 2 + Kabob_Code/src/Test/Test.cpp | 8 ++- 5 files changed, 53 insertions(+), 33 deletions(-) diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp index 7d598e5..8d38c67 100644 --- a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp +++ b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp @@ -3,11 +3,6 @@ void MotorWithEncoder::activity(){ pos = abs(encoder.read()); - - Serial.println("position = "); - Serial.println(pos); - Serial.println("target : "); - Serial.println(target); // overflow protection if (pos > 0xFFFFFF){ @@ -29,9 +24,13 @@ void MotorWithEncoder::stop(void) { overflow = 0; pos = 0; target = 0; + occupied = false; } void MotorWithEncoder::move(double angle, uint8_t speed){ + if (occupied) return; + occupied = true; + if (angle > 0){ cw_at_speed(speed); } else { diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.h b/Kabob_Code/src/Actuators/MotorWithEncoder.h index 83b7a35..1203d28 100644 --- a/Kabob_Code/src/Actuators/MotorWithEncoder.h +++ b/Kabob_Code/src/Actuators/MotorWithEncoder.h @@ -42,4 +42,5 @@ class MotorWithEncoder : public Motor double target; double overflow; uint8_t min_speed; + uint8_t occupied; }; \ No newline at end of file diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 991028d..d46dea6 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -2,8 +2,10 @@ #define VELOCITY 100 -#define Right_TURN_TIME 2100 -#define SECOUND_TURN_TIME 550 +#define Right_TURN_TIME 3000 +#define SECOUND_TURN_TIME 1000 + +uint8_t DEBUG = true; States_t state; States_r line_state; @@ -20,6 +22,10 @@ uint8_t servoPos = 0; uint8_t flagLeftLine = 0; uint8_t flagRightLine = 0; + +// right now code only works for state red +uint8_t stateRed = true; + //if this is true, do not allow motor to be set on different course // unless extreme reason (i.e. hole) uint8_t navigationSet = 0; @@ -31,16 +37,14 @@ void setup() { Serial.begin(9600); shephard.claw.open(); while(!Serial); - //delay(10000); - delay(3000); + delay(5000); + //times curr_time = millis(); serial_time = millis(); state_time = millis(); flag_time = millis(); - - changeStateTo(STATE_LOAD); Serial.println("Setup Complete!"); @@ -51,9 +55,7 @@ void loop() { checkGlobalEvents(); checkFlags(); checkForZoneChange(); - shephard.chassis.activity(); - - // TO LINE FOLLOW: Must be in state = state_nav_target + shephard.activity(); switch(state) { case STATE_IDLE: @@ -76,14 +78,14 @@ void loop() { } //debug for line sensors - if (curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){ + if (curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) && DEBUG){ + Serial.println("---------------"); + Serial.println("left " + String(shephard.sensors.line.left.read())); + Serial.println("right " + String(shephard.sensors.line.right.read())); + Serial.println("center left " + String(shephard.sensors.line.center_left.read())); + Serial.println("center middle " + String(shephard.sensors.line.center_middle.read())); + Serial.println("center right " + String(shephard.sensors.line.center_right.read())); Serial.println("---------------"); - //Serial.println("left " + String(shephard.sensors.line.left.read())); - //Serial.println("right " + String(shephard.sensors.line.right.read())); - //Serial.println("center left " + String(shephard.sensors.line.center_left.read())); - //Serial.println("center middle " + String(shephard.sensors.line.center_middle.read())); - //Serial.println("center right " + String(shephard.sensors.line.center_right.read())); - //Serial.println("---------------"); serial_time = curr_time; } } @@ -94,9 +96,9 @@ void handleLoadState(void) { shephard.claw.open(); } else if (timeInState > 1000 && timeInState < 2000) { shephard.claw.close(); - } else if (timeInState > 2000 && timeInState < 2500) { - shephard.chassis.move_forward_at_speed(150); - } else if (timeInState > 2500) { + } else if (timeInState > 2000 && timeInState < 2100) { + shephard.chassis.move_forward(20, 150); + } else if (timeInState > 3500) { changeStateTo(STATE_NAV_TARGET); changeZoneTo(ZONE_1); } @@ -107,9 +109,9 @@ void handleUnloadState(void) { if (timeInState < 1000) { shephard.claw.open(); } else if (timeInState > 1000 && timeInState < 4000) { - shephard.chassis.move_backward_at_speed(VELOCITY); + shephard.chassis.move_backward(15, VELOCITY); } else if (timeInState > 4000 && timeInState < 10000) { - shephard.chassis.turn_left_at_speed(VELOCITY); + shephard.chassis.turn_ccw(360, VELOCITY); } else if (timeInState > 10000) { changeStateTo(STATE_NAV_LOAD); } @@ -132,17 +134,17 @@ void handleNavTargetState(void){ break; case ZONE_3: // turn 90 degrees and then line follow - if (t > 300 && t < 300+Right_TURN_TIME) { - shephard.chassis.turn_right(100); - } else { + if (t > 300 && t < 300+300) { + makeFirstTurn(); + } else if (t < 4000) { lineFollow(); } break; case ZONE_4: // turn less than 90 degrees and then line follow - if (t < SECOUND_TURN_TIME) { - shephard.chassis.turn_left(100); - } else { + if (t < 100) { + shephard.chassis.turn_ccw(100); + } else if (t > 4000) { lineFollow(); } break; @@ -234,6 +236,16 @@ void lineFollow(void) { } } + +void makeFirstTurn() { + if (stateRed) shephard.chassis.turn_cw(90, 100); + else shephard.chassis.turn_ccw(90, 100); +} + +void makeSecondTurn() { + if (stateRed) shephard.chassis.turn_ccw(70, 100); + else shephard.chassis.turn_ccw(70, 100); +} uint8_t TestForKey(void) { uint8_t KeyEventOccurred; diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index 2d83038..c73b5a2 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -46,5 +46,7 @@ void changeLineStateTo(States_r s); void changeStateTo(States_t s); void changeZoneTo(Zones_t z); void setFlag(uint8_t flag); +void makeSecondTurn(void); +void makeFirstTurn(void); /*---------------Module Variables---------------------------*/ diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index 502c0b3..5fce1ec 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -1,13 +1,19 @@ #include "Test.h" +unsigned long start; void setup(){ Serial.begin(9600); while(!Serial); Serial.println("Starting"); - shephard.chassis.turn_ccw(90, 150); + start = millis(); } void loop(void){ + Serial.println(millis() - start); + if (millis() - start < 500) { + Serial.println("test"); + shephard.chassis.move_forward(20, 150); + } shephard.activity(); delay(100); } \ No newline at end of file From 2a8b6479efaaafd8e251ecd9041965160a02e759 Mon Sep 17 00:00:00 2001 From: Flynn Dreilinger Date: Sat, 5 Mar 2022 03:14:51 -0800 Subject: [PATCH 4/7] move demo code to markdown file --- Kabob_Code/src/Test/Test.cpp | 18 ------------ Kabob_Code/src/Test/Test.h | 43 +--------------------------- Kabob_Code/src/Test/code_snippets.md | 22 ++++++++++++++ 3 files changed, 23 insertions(+), 60 deletions(-) diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index 5fce1ec..77ba015 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -1,19 +1 @@ #include "Test.h" - -unsigned long start; -void setup(){ - Serial.begin(9600); - while(!Serial); - Serial.println("Starting"); - start = millis(); -} - -void loop(void){ - Serial.println(millis() - start); - if (millis() - start < 500) { - Serial.println("test"); - shephard.chassis.move_forward(20, 150); - } - shephard.activity(); - delay(100); -} \ No newline at end of file diff --git a/Kabob_Code/src/Test/Test.h b/Kabob_Code/src/Test/Test.h index db19715..25a4b15 100644 --- a/Kabob_Code/src/Test/Test.h +++ b/Kabob_Code/src/Test/Test.h @@ -1,45 +1,4 @@ #pragma once #include -#include -#include "System.h" - - - -typedef enum { - STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD, - STATE_NAV_LOAD, -} States_t; - -typedef enum { - STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT, - STATE_OFF_SLIGHT_RIGHT, STATE_OFF_SLIGHT_LEFT -} States_r; - -// Zone defintions -typedef enum { - ZONE_LOAD, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET -} Zones_t; - - -void checkGlobalEvents(void); -void checkForZoneChange(void); -void lineFollow(void); -void handleMoveForward(void); -void handleMoveBackward(void); -uint8_t TestForKey(void); -void RespToKey(void); -void handleRightTurn(void); -void handleLeftTurn(void); -void handleLoadState(void); -void handleUnloadState(void); -void handleNavTargetState(void); -void handleNavLoadState(void); -void changeStateTo(States_t); -void setup(void); -void loop(void); -void checkFlags(void); -void changeLineStateTo(States_r s); -void changeStateTo(States_t s); -void changeZoneTo(Zones_t z); -void setFlag(uint8_t flag); \ No newline at end of file +#include \ No newline at end of file diff --git a/Kabob_Code/src/Test/code_snippets.md b/Kabob_Code/src/Test/code_snippets.md index 0fb0995..e2a05d3 100644 --- a/Kabob_Code/src/Test/code_snippets.md +++ b/Kabob_Code/src/Test/code_snippets.md @@ -162,3 +162,25 @@ void loop(void){ } } ``` + +### Encoder Funcionality + +```c++ +unsigned long start; +void setup(){ + Serial.begin(9600); + while(!Serial); + Serial.println("Starting"); + start = millis(); +} + +void loop(void){ + Serial.println(millis() - start); + if (millis() - start < 500) { + Serial.println("test"); + shephard.chassis.move_forward(20, 150); + } + shephard.activity(); + delay(100); +} +``` \ No newline at end of file From 598c8e8d59d1a30ae9697ba5d1528f75ccb90cbf Mon Sep 17 00:00:00 2001 From: Flynn Dreilinger Date: Sat, 5 Mar 2022 03:16:19 -0800 Subject: [PATCH 5/7] simplify calculation of desired wheel movements --- Kabob_Code/src/Actuators/Chassis.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Kabob_Code/src/Actuators/Chassis.cpp b/Kabob_Code/src/Actuators/Chassis.cpp index 112cdc9..d180921 100644 --- a/Kabob_Code/src/Actuators/Chassis.cpp +++ b/Kabob_Code/src/Actuators/Chassis.cpp @@ -47,14 +47,14 @@ void Chassis::veer_backward(uint8_t speed_right, uint8_t speed_left) void Chassis::turn_cw(double angle, uint8_t speed) { - double movement_degrees = chassis_circumference * (angle / 360) / wheel_circumference * 360; + double movement_degrees = angle * chassis_circumference / wheel_circumference; right.ccw(movement_degrees, speed); left.ccw(movement_degrees, speed); } void Chassis::turn_ccw(double angle, uint8_t speed) { - double movement_degrees = chassis_circumference * (angle / 360) / wheel_circumference * 360; + double movement_degrees = angle * chassis_circumference / wheel_circumference; right.cw(movement_degrees, speed); left.cw(movement_degrees, speed); } From 5cbe1a3137ad7208c7f560225788081721989566 Mon Sep 17 00:00:00 2001 From: Flynn Dreilinger Date: Sat, 5 Mar 2022 03:19:14 -0800 Subject: [PATCH 6/7] substitute similar method for motor movement --- Kabob_Code/src/Actuators/Chassis.cpp | 8 ++++---- Kabob_Code/src/Actuators/MotorWithEncoder.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Kabob_Code/src/Actuators/Chassis.cpp b/Kabob_Code/src/Actuators/Chassis.cpp index d180921..9921d30 100644 --- a/Kabob_Code/src/Actuators/Chassis.cpp +++ b/Kabob_Code/src/Actuators/Chassis.cpp @@ -10,15 +10,15 @@ void Chassis::stop(void) void Chassis::move_forward(double distance, uint8_t speed) { double movement_degrees = distance / wheel_circumference * 360; - right.move(movement_degrees, speed); - left.move(-movement_degrees, speed); + right.cw(movement_degrees, speed); + left.ccw(-movement_degrees, speed); } void Chassis::move_backward(double distance, uint8_t speed) { double movement_degrees = distance / wheel_circumference * 360; - right.move(-movement_degrees, speed); - left.move(movement_degrees, speed); + right.cw(-movement_degrees, speed); + left.ccw(movement_degrees, speed); } void Chassis::turn_right(uint8_t speed) diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp index 8d38c67..07c0e1f 100644 --- a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp +++ b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp @@ -1,7 +1,7 @@ #include "Arduino.h" #include "MotorWithEncoder.h" -void MotorWithEncoder::activity(){ +void MotorWithEncoder::activity(){ //TODO schedule with interrupt at some frequency pos = abs(encoder.read()); // overflow protection From 14f0a8aff21f7f9c7586aacbe155828c5d0e4a4e Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Sat, 5 Mar 2022 18:09:05 -0800 Subject: [PATCH 7/7] paxton stepper code --- Kabob_Code/platformio.ini | 1 + Kabob_Code/src/Actuators/Chassis.h | 8 +++++- Kabob_Code/src/Actuators/StepperMotor.cpp | 17 +++++++++++++ Kabob_Code/src/Actuators/StepperMotor.h | 25 +++++++++++++++++++ Kabob_Code/src/System.h | 8 +++++- Kabob_Code/src/Test/Test.cpp | 30 +++++++++++++++++++++++ 6 files changed, 87 insertions(+), 2 deletions(-) create mode 100644 Kabob_Code/src/Actuators/StepperMotor.cpp create mode 100644 Kabob_Code/src/Actuators/StepperMotor.h diff --git a/Kabob_Code/platformio.ini b/Kabob_Code/platformio.ini index 95d9947..e4181f5 100644 --- a/Kabob_Code/platformio.ini +++ b/Kabob_Code/platformio.ini @@ -18,6 +18,7 @@ board = teensylc framework = arduino lib_deps = https://github.com/PaulStoffregen/Encoder.git + arduino-libraries/Stepper@^1.1.3 lib_ignore = FreeRTOS-Kernel diff --git a/Kabob_Code/src/Actuators/Chassis.h b/Kabob_Code/src/Actuators/Chassis.h index 76587a0..34a4f6a 100644 --- a/Kabob_Code/src/Actuators/Chassis.h +++ b/Kabob_Code/src/Actuators/Chassis.h @@ -3,6 +3,7 @@ #include "Arduino.h" #include "Motor.h" #include "MotorWithEncoder.h" +#include "StepperMotor.h" class Chassis { @@ -17,8 +18,11 @@ class Chassis unsigned le_pin2, double hub_to_hub_distance, double wheel_circumference, - uint8_t min_speed) : right(rf_pin, rb_pin, re_pin1, re_pin2, min_speed), + uint8_t min_speed, + unsigned dir_pin, + unsigned step_pin) : right(rf_pin, rb_pin, re_pin1, re_pin2, min_speed), left(lf_pin, lb_pin, le_pin1, le_pin2, min_speed), + stepper(dir_pin, step_pin), chassis_circumference(hub_to_hub_distance * PI), wheel_circumference(wheel_circumference), ninety_degrees(chassis_circumference / 4 / wheel_circumference * 360){}; @@ -57,6 +61,8 @@ class Chassis MotorWithEncoder right; MotorWithEncoder left; + StepperMotor stepper; + private: double chassis_circumference, wheel_circumference, diff --git a/Kabob_Code/src/Actuators/StepperMotor.cpp b/Kabob_Code/src/Actuators/StepperMotor.cpp new file mode 100644 index 0000000..e450f30 --- /dev/null +++ b/Kabob_Code/src/Actuators/StepperMotor.cpp @@ -0,0 +1,17 @@ +#include "Arduino.h" +#include "StepperMotor.h" + +void StepperMotor::cw(int steps) { + stepper.setSpeed(60); + stepper.step(steps); +} + +void StepperMotor::ccw(int steps) { + stepper.setSpeed(-60); + stepper.step(steps); +} + +void StepperMotor::stop() { + return; +} + diff --git a/Kabob_Code/src/Actuators/StepperMotor.h b/Kabob_Code/src/Actuators/StepperMotor.h new file mode 100644 index 0000000..4d3a99d --- /dev/null +++ b/Kabob_Code/src/Actuators/StepperMotor.h @@ -0,0 +1,25 @@ +#pragma once + +#include "Arduino.h" +#include "Stepper.h" + +#define STEPS_PER_REVOLUTION 200 + +class StepperMotor +{ +public: + StepperMotor(unsigned dir_pin, unsigned step_pin) + { + pinMode(dir_pin, OUTPUT); + pinMode(step_pin, OUTPUT); + stepper = Stepper(STEPS_PER_REVOLUTION, dir_pin, step_pin); + + }; + + void cw(int steps); + void ccw(int steps); + void stop(void); + + Stepper stepper; + +}; diff --git a/Kabob_Code/src/System.h b/Kabob_Code/src/System.h index c13ba6c..8123da7 100644 --- a/Kabob_Code/src/System.h +++ b/Kabob_Code/src/System.h @@ -27,11 +27,15 @@ #define LINE_CENTER_RIGHT 16 #define LINE_CENTER_LEFT 17 +#define STEPPER_DIR 16 +#define STEPPER_STEP 15 + #define HUB_TO_HUB_DISTANCE 25.1 // cm #define WHEEL_CIRCUMFERENCE 31.33 // cm #define MOTOR_MIN_SPEED 55 // Measured with both motors running. // If only 1 motor is running the min will be higher + struct System { public: @@ -65,7 +69,9 @@ struct System ENCODER_LEFT_PIN2, HUB_TO_HUB_DISTANCE, WHEEL_CIRCUMFERENCE, - MOTOR_MIN_SPEED); + MOTOR_MIN_SPEED, + STEPPER_DIR, + STEPPER_STEP); Claw claw = Claw(SERVO_PIN, OPEN_CLAW_ANGLE, CLOSE_CLAW_ANGLE); void activity(void); diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index 77ba015..4728661 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -1 +1,31 @@ #include "Test.h" +#include "System.h" +#include "Stepper.h" + +// change this to the number of steps on your motor +//Stepper stepper2 = Stepper(200, 16, 15); +// the previous reading from the analog input +int previous = 0; + + +void setup() { + // set the speed of the motor to 30 RPMs + //stepper2.setSpeed(60); + +} + +void loop() { + // get the sensor value + + // move a number of steps equal to the change in the + // sensor reading + shephard.chassis.stepper.cw(100); + + //stepper2.step(100); + delay(1000); + + Serial.println("test"); + + + // remember the previous value of the sensor +} \ No newline at end of file