Move back to encoders

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
 

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.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.ccw_at_speed(speed);
-  left.cw_at_speed(speed);
+  right.cw(-movement_degrees, speed);
+  left.ccw(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 = 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;
-  right.cw_at_speed(speed);
-  left.cw_at_speed(speed);
+  double movement_degrees = angle * chassis_circumference / wheel_circumference;
+  right.cw(movement_degrees, speed);
+  left.cw(movement_degrees, speed);
 }
 
 void Chassis::move_forward_at_speed(uint8_t speed)

Kabob_Code/src/Actuators/Chassis.h

@@ -2,6 +2,8 @@
 
 #include "Arduino.h"
 #include "Motor.h"
+#include "MotorWithEncoder.h"
+#include "StepperMotor.h"
 
 class Chassis
 {
@@ -16,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, min_speed),
-                               left(lf_pin, lb_pin, 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){};
@@ -29,14 +34,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 +50,22 @@ 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;
+
+  StepperMotor stepper;
 
 private:
   double chassis_circumference,
       wheel_circumference,
       ninety_degrees;
+  uint8_t executing;
 };

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
@@ -24,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 {

Kabob_Code/src/Actuators/MotorWithEncoder.h

@@ -6,7 +6,7 @@
 #define ENCODER_OPTIMIZE_INTERRUPTS
 #include <Encoder.h>
 
-class MotorWithEncoder : Motor
+class MotorWithEncoder : public Motor
 {
 public:
   MotorWithEncoder(unsigned f_pin,
@@ -42,4 +42,5 @@ class MotorWithEncoder : Motor
   double target;
   double overflow;
   uint8_t min_speed;
+  uint8_t occupied;
 };

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;
+}
+

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;
+
+};

Kabob_Code/src/StateMachine/StateMachine.cpp

@@ -1,9 +1,11 @@
 #include "StateMachine.h"
 
-#define VELOCITY 255
+#define VELOCITY 100
 
-#define Right_TURN_TIME       4000
-#define SECOUND_TURN_TIME     2500
+#define Right_TURN_TIME       3000
+#define SECOUND_TURN_TIME     1000
+
+uint8_t DEBUG = true; 
 
 States_t state;
 States_r line_state;
@@ -20,23 +22,29 @@ 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;
+
 unsigned long flag_time;
 
 void setup() {
   // put your setup code here, to run once:
   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!");
@@ -47,8 +55,7 @@ void loop() {
   checkGlobalEvents();
   checkFlags();
   checkForZoneChange();
-
-  // TO LINE FOLLOW: Must be in state = state_nav_target
+  shephard.activity();
 
   switch(state) {
     case STATE_IDLE:
@@ -71,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;
   }
 }
@@ -89,9 +96,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 < 2100) {
+    shephard.chassis.move_forward(20, 150);
+  } else if (timeInState > 3500) { 
     changeStateTo(STATE_NAV_TARGET);
     changeZoneTo(ZONE_1);
   }
@@ -102,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);
   } 
@@ -127,22 +134,22 @@ 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);
-      } 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(200);
-      } else {
+      if (t < 100) {
+        shephard.chassis.turn_ccw(100);
+      } else if (t > 4000) {
         lineFollow();
       }
       break;
     case ZONE_TARGET:
-      if (t < 1000) {
+      if (t < 400) {
         shephard.chassis.move_forward_at_speed(VELOCITY);
       } else {
         changeStateTo(STATE_UNLOAD);
@@ -178,10 +185,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,25 +217,35 @@ 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");
     }
 
 }
+
+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;