diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino index 87b7ecc6895cc4928e1f52a6a4d8d52ef53b6838..1d90acb905027a3070c11729fd7b4fc2cfcfd211 100644 --- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino +++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino @@ -16,8 +16,17 @@ // - To maintain constant force sign, we want the stepper's internal phase to be ahead/behind the position phase. -// BROKEN!!! -// - Seems to get stuck, possibly losing sync? (thinks the phases are set to move the motor, but no movement) +// Starting from a zero rest state +// - Set the coils ahead +// > When count goes below -5, need to switch backwards one step (pulling towards zero) +// > 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? +// IDEA: In final build, have a self-calibration routine that moves the motor with no load to calibrate/reset state // NOTE: // - Encoder is 1000 pulse (4000 counts) per revolution @@ -37,7 +46,8 @@ const int OUTPUT_CLK1 = 13; const int INPUT_VREF1 = 15; // A1 const int INPUT_POT1 = 18; -int last_1, val_1; +volatile int last_1; +volatile int val_1; int ctrl_phase_1; // Current "phase" of the controller (0-3) int axle_phase_1; // Current "phase" of the axle (0-3) @@ -55,9 +65,11 @@ void step_once_1(int dir) { ctrl_phase_1 = phase_add(ctrl_phase_1, dir < 0 ? -1 : 1); digitalWrite(OUTPUT_DIR1, dir < 0); + delayMicroseconds(1); digitalWrite(OUTPUT_CLK1, 1); - delay(1); + delayMicroseconds(1); digitalWrite(OUTPUT_CLK1, 0); + delayMicroseconds(1); } void setup() @@ -93,24 +105,61 @@ int step_count; // Number of steps performed since last log int last_val1; // Last value of `val_1` seen int err_count; // Number of times the phases have been out by 180 degrees int loop_count; +int backdrive_count; +int TARGET_DIR = 1; +int s_run = 1; +int last_seen_encoder; void loop() { int should_log = 0; + if( Serial.available() ) { + int b = Serial.read(); + int upd = 1; + switch(b) + { + case 'w': axle_phase_1 = phase_add(axle_phase_1, 1); break; + case 's': axle_phase_1 = phase_add(axle_phase_1, -1); break; + case 'a': TARGET_DIR = (TARGET_DIR >= 0 ? 1 : 0); break; + case 'd': TARGET_DIR = (TARGET_DIR <= 0 ? -1 : 0); break; + case 'p': s_run = s_run ? 0 : 1; break; + //case 'o': s_run = !s_run; break; + case ' ': break; + default: + upd = 0; + break; + } + if(upd) { + Serial.print("axle_phase_1="); Serial.print(axle_phase_1); + Serial.print("TARGET_DIR="); + Serial.println(TARGET_DIR); + should_log = 1; + } + } + + // Monitor the encoder value, and see if it exceeds +/- 5 // - When it does, adjust back into range and update the current axle phase - int v = val_1; - if( v <= -5 ) { + int v0 = val_1; + int v = v0; + while( v <= -6 ) { val_1 += 10; + v += 10; axle_phase_1 = phase_add(axle_phase_1, -1); - should_log = 1; + //should_log = 1; } - else if( v >= 5 ) { + while( v >= 6 ) { val_1 -= 10; + v -= 10; axle_phase_1 = phase_add(axle_phase_1, +1); - should_log = 1; + //should_log = 1; } - else { - // In-range + // In-range + + if( last_seen_encoder != v0 ) { + last_seen_encoder = v0; + if( !s_run ){ + should_log = true; + } } // Compare `ctrl_phase_1` and `real_phase_1` to determine the current "mode" @@ -119,21 +168,28 @@ 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 - static const int TARGET_DIR = 1; int 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); - if( phase_diff == 0 ) { + int phase_diff = phase_add(cur_ctrl_phase_1, -target_ctrl_phase_1); + if( s_run == 0 ) { + } + else if( phase_diff == 0 ) { // No need to change } else if( phase_diff == 3 ) { // Need to shift phase up + if( TARGET_DIR < 0 ) { + backdrive_count += 1; + } step_once_1(1); step_count += 1; } else if( phase_diff == 1 ) { // Need to shift phase down + if( TARGET_DIR > 0 ) { + backdrive_count += 1; + } step_once_1(-1); step_count += 1; } @@ -141,8 +197,8 @@ void loop() // Going opposite! - Run twice //step_once_1(1); //step_once_1(1); + //step_count += 2; err_count += 1; - step_count += 2; } loop_count += 1; @@ -155,14 +211,18 @@ void loop() // - Current phase values // - 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 ) { - if( true /*Serial.availableForWrite() > 20*/ ) { + if( /*step_count > 0 ||*/ err_count > 0 || should_log /*|| phase_diff != 0*/ ) { + if( !s_run /*Serial.availableForWrite() > 20*/ ) { Serial.print(step_count); + 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); Serial.print(" C:"); Serial.print(cur_ctrl_phase_1); Serial.print("-"); Serial.print(target_ctrl_phase_1); - Serial.print(" D="); Serial.print(phase_diff); + Serial.print("="); Serial.print(phase_diff); + Serial.print(" D="); Serial.print(TARGET_DIR); + Serial.print(s_run); Serial.println(""); step_count = 0; err_count = 0; diff --git a/Arduino/test_r2_encoder/test_r2_encoder.ino b/Arduino/test_r2_encoder/test_r2_encoder.ino index b4617c8b5c08f243659f51263fbf4247dc96e9e1..1849f3b8535f1d10d61a83072c0b4dfe9f24b2d3 100644 --- a/Arduino/test_r2_encoder/test_r2_encoder.ino +++ b/Arduino/test_r2_encoder/test_r2_encoder.ino @@ -32,6 +32,10 @@ void setup() pinMode(OUTPUT_CLK1, OUTPUT); pinMode(PIN_LED, OUTPUT); + delay(5); + digitalWrite(OUTPUT_MOT_ON, 1); + delay(100); + attachInterrupt(0, isr_enc1, CHANGE); attachInterrupt(1, isr_enc1, CHANGE); } @@ -63,8 +67,16 @@ void isr_enc1() { } } int last_seen_1; +int step_count; +long count; void loop() { + if( val_1 >= 4000 ) { + val_1 -= 4000; + } + if( val_1 < 0 ) { + val_1 += 4000; + } if( val_1 != last_seen_1 ) { Serial.print("Encoder 1: "); int v = val_1; @@ -72,6 +84,7 @@ void loop() Serial.print("->"); Serial.print(v); Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")"); + Serial.print(step_count); Serial.println(""); last_seen_1 = v; } @@ -89,4 +102,12 @@ void loop() } Serial.print("Encoder 2: "); Serial.println(val_2); } + count += 1; + if( count > 80000 ) { + count = 0; + step_count += 1; + digitalWrite(OUTPUT_CLK1, 1); + delay(1); + digitalWrite(OUTPUT_CLK1, 0); + } }