diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
index 7e4db829677cccababf5277d546a3bbe93529db6..d2111e7b8ac0b194d292d5df618439153fe1c206 100644
--- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino
+++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
@@ -22,10 +22,8 @@
// > When count goes above 5, switch forwards (pushing positive)
// STATUS:
-// - Sometimes moves well, sometimes stops
-// - When moving, the motor can't be reversed.
-// - When it stops, need to override step count to get it moving again.
-// - Might be a miscount of encoder ticks?
+// - Can end up slightly offset from the proper center.
+// - Sometimes stops when it shouldn't :(
// IDEA: In final build, have a self-calibration routine that moves the motor with no load to calibrate/reset state
// NOTE:
@@ -38,21 +36,22 @@
#define INPUT_ENC1A 2
#define INPUT_ENC1B 3
-const int OUTPUT_VREF1 = 5; // D5
-const int OUTPUT_VREF2 = 6; // D6
+#define OUTPUT_VREF1 5 // D5
+#define OUTPUT_VREF2 6 // D6
const int OUTPUT_MOT_ON = 9;
const int OUTPUT_DIR1 = 12;
const int OUTPUT_CLK1 = 13;
const int INPUT_VREF1 = 15; // A1
const int INPUT_POT1 = 18;
-volatile int val_1;
+volatile int val_1; // Encoder counter
+volatile int val_1_raw; // Encoder counter (not clamped)
-int ctrl_phase_1; // Current "phase" of the controller (0-3)
-int axle_phase_1; // Current "phase" of the axle (0-3)
+signed char ctrl_phase_1; // Current "phase" of the controller (0-3)
+signed char axle_phase_1; // Current "phase" of the axle (0-3)
// Addition modulo 4
-static int phase_add(int v, int i) {
+static signed char phase_add(signed char v, signed char i) {
v += i;
while(v > 3) v -= 4;
while(v < 0) v += 4;
@@ -78,7 +77,6 @@ void setup()
pinMode(OUTPUT_MOT_ON, OUTPUT);
pinMode(OUTPUT_DIR1, OUTPUT);
pinMode(OUTPUT_CLK1, OUTPUT);
-// pinMode(PIN_LED, OUTPUT);
init_isr_enc1();
@@ -110,6 +108,7 @@ int last_seen_encoder;
void loop()
{
int should_log = 0;
+ // Serial input
if( Serial.available() ) {
int b = Serial.read();
int upd = 1;
@@ -128,7 +127,7 @@ void loop()
}
if(upd) {
Serial.print("axle_phase_1="); Serial.print(axle_phase_1);
- Serial.print("TARGET_DIR=");
+ Serial.print(" TARGET_DIR=");
Serial.println(TARGET_DIR);
should_log = 1;
}
@@ -152,7 +151,8 @@ void loop()
//should_log = 1;
}
// In-range
-
+
+ // Has the encoder moved?
if( last_seen_encoder != v0 ) {
last_seen_encoder = v0;
if( !s_run ){
@@ -166,11 +166,12 @@ void loop()
// - Opposite: Bad - unstable version of holding configuration. Step once to get direction going the right way again.
// Get a target `ctrl_phase_1` based on `real_phase_1` and a target direction
- int target_ctrl_phase_1 = phase_add(axle_phase_1, TARGET_DIR);
+ signed char target_ctrl_phase_1 = phase_add(axle_phase_1, TARGET_DIR);
- int cur_ctrl_phase_1 = ctrl_phase_1;
- int phase_diff = phase_add(cur_ctrl_phase_1, -target_ctrl_phase_1);
+ signed char cur_ctrl_phase_1 = ctrl_phase_1;
+ signed char phase_diff = phase_add(cur_ctrl_phase_1, -target_ctrl_phase_1);
if( s_run == 0 ) {
+ // Not running, don't do any steps
}
else if( phase_diff == 0 ) {
// No need to change
@@ -210,9 +211,10 @@ void loop()
// - Total number of steps taken since last log
// - Number of times the phases have ended up opposed
if( /*step_count > 0 ||*/ err_count > 0 || should_log /*|| phase_diff != 0*/ ) {
- if( !s_run /*Serial.availableForWrite() > 20*/ ) {
+ if( true || !s_run /*Serial.availableForWrite() > 20*/ ) {
Serial.print(step_count);
- Serial.print(" "); Serial.print(last_seen_encoder);
+ Serial.print(" ");
+ Serial.print(last_seen_encoder);
Serial.print(" E="); Serial.print(err_count);
Serial.print(" B="); Serial.print(backdrive_count);
Serial.print(" A:"); Serial.print(axle_phase_1);
@@ -220,7 +222,8 @@ void loop()
Serial.print("-"); Serial.print(target_ctrl_phase_1);
Serial.print("="); Serial.print(phase_diff);
Serial.print(" D="); Serial.print(TARGET_DIR);
- Serial.print(s_run);
+ Serial.print(" R="); Serial.print(s_run);
+ Serial.print(" "); Serial.print(val_1_raw);
Serial.println("");
step_count = 0;
err_count = 0;
@@ -233,10 +236,10 @@ const char E = -99;
// - low bits are current value
// - high bits are previous value
const char GREY_MAPPING[16] = {
- 0, -1, 1, E,
- 1, 0, E, -1,
- -1, E, 0, 1,
- E, 1, -1, 0,
+ 0, -1, 1, E, // LL -> {LL, LH, HL, HH}
+ 1, 0, E, -1, // LH -> {LL, LH, HL, HH}
+ -1, E, 0, 1, // HL -> {LL, LH, HL, HH}
+ E, 1, -1, 0, // HH -> {LL, LH, HL, HH}
};
@@ -251,19 +254,28 @@ void init_isr_enc1() {
pinport_ENC1B = portInputRegister(digitalPinToPort(INPUT_ENC1B));
attachInterrupt(0, isr_enc1, CHANGE);
attachInterrupt(1, isr_enc1, CHANGE);
+
+ //Serial.print("pinmask_ENC1A = "); Serial.print(pinmask_ENC1A); Serial.print(" ");
+ //Serial.print("pinport_ENC1A = "); Serial.print((uint32_t)pinport_ENC1A,HEX); Serial.println("");
+ //Serial.print("pinmask_ENC1B = "); Serial.print(pinmask_ENC1B); Serial.print(" ");
+ //Serial.print("pinport_ENC1B = "); Serial.print((uint32_t)pinport_ENC1B,HEX); Serial.println("");
}
+
+// Encoder edge interrupt
+// Update the counters based on the transition type (see GREY_MAPPING)
volatile char last_1;
void isr_enc1() {
uint8_t pinValue_ENC1A = (*pinport_ENC1A & pinmask_ENC1A) != 0;
uint8_t pinValue_ENC1B = (*pinport_ENC1B & pinmask_ENC1B) != 0;
- char v = (char)pinValue_ENC1A << 1 + (char)pinValue_ENC1B;
+ char v = ((char)pinValue_ENC1A << 1) + (char)pinValue_ENC1B;
if( v != last_1 ) {
char adj = GREY_MAPPING[ (char)(last_1 << 2) + v ];
if(adj == E) {
}
else {
val_1 += adj;
+ val_1_raw += adj;
}
last_1 = v;
}