diff --git a/Arduino/ff_main/ff_main.ino b/Arduino/ff_main/ff_main.ino new file mode 100644 index 0000000000000000000000000000000000000000..8198ba5b0a9b786496241143f7a9edfbfd884eca --- /dev/null +++ b/Arduino/ff_main/ff_main.ino @@ -0,0 +1,90 @@ +/* + ForceFeedback main + */ +const int PIN_CHA_O_DIR = 2; //D2; +const int PIN_CHA_O_VRF = 3; //D3; +const int PIN_CHA_O_CLK = 4; //D4; +const int PIN_CHB_O_DIR = 5; //D5; +const int PIN_CHB_O_VRF = 6; //D6; +const int PIN_CHB_O_CLK = 7; //D7; +const int PIN_CHA_I_POT = 14; //A0; +const int PIN_CHA_I_ENC = 15; //A1; +const int PIN_CHA_I_VRF = 16; //A2; +const int PIN_CHB_I_POT = 17; //A3; +const int PIN_CHB_I_ENC = 18; //A4; +const int PIN_CHB_I_VRF = 19; //A5; +const int PIN_LED = 13; // Orange LED + +int prev_encoder_a; +int prev_encoder_b; +int v; // Temporary +int counter; +int raw_a; +int prev_raw_a; + +void setup() +{ + Serial.begin(9600); + pinMode(PIN_CHA_O_DIR, OUTPUT); + pinMode(PIN_CHA_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHA_O_CLK, OUTPUT); + pinMode(PIN_CHB_O_DIR, OUTPUT); + pinMode(PIN_CHB_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHB_O_CLK, OUTPUT); + pinMode(PIN_LED, OUTPUT); +} + +void loop() +{ + if(true) { + raw_a = analogRead(PIN_CHA_I_ENC); + if( abs(raw_a - prev_raw_a) > 10 ) { + Serial.print("A "); Serial.print(raw_a); Serial.println(""); + prev_raw_a = raw_a; + } + } + if( false ) { + raw_a = analogRead(PIN_CHA_I_ENC); + v = map(raw_a, 0, 1024, 0, 15); + if( v != prev_encoder_a ) { + Serial.print("A "); + Serial.print(prev_encoder_a); + Serial.print(" -> "); + Serial.print(v); + Serial.print(": "); + Serial.print(raw_a); + Serial.println(""); + prev_encoder_a = v; + } + } + + if(false) { + v = map(analogRead(PIN_CHB_I_ENC), 0, 1023, 0, 15); + if( v != prev_encoder_b ) { + Serial.print("B "); + Serial.print(prev_encoder_b); + Serial.print(" -> "); + Serial.println(v); + prev_encoder_b = v; + } + } + + // Blink a LED at 500ms/500ms + counter += 1; + if( counter == 50 ) { + digitalWrite(PIN_LED, HIGH); + } + if(counter >= 100) { + digitalWrite(PIN_LED, LOW); + counter = 0; + if(false) { + Serial.print("RAW "); + Serial.print(analogRead(PIN_CHA_I_ENC)); + Serial.print(" "); + Serial.print(analogRead(PIN_CHB_I_ENC)); + Serial.println(""); + } + } + delay(10); +} + diff --git a/Arduino/test_analog/test_analog.ino b/Arduino/test_analog/test_analog.ino new file mode 100644 index 0000000000000000000000000000000000000000..36dd62c357c613c543530b6bc19232e17adf603e --- /dev/null +++ b/Arduino/test_analog/test_analog.ino @@ -0,0 +1,50 @@ +/* + TEST: DAC with feedback. + */ + +const int PIN_CHA_O_VRF = 3; +const int PIN_CHB_O_VRF = 6; +const int PIN_CHA_I_VRF = 16; //A2; +const int PIN_CHB_I_VRF = 19; //A5 +const int PIN_LED = 13; // Orange LED + +void setup() +{ + Serial.begin(9600); + pinMode(PIN_CHA_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHB_O_VRF, OUTPUT); // PWM + pinMode(PIN_LED, OUTPUT); +} + +int step_val; +int cnt; +void loop() +{ + Serial.print(step_val); + Serial.print(" "); + analogWrite(PIN_CHB_O_VRF, step_val); + analogWrite(PIN_CHA_O_VRF, step_val); + delay(50); + Serial.print(analogRead(PIN_CHA_I_VRF)); + Serial.print(" "); + Serial.print(analogRead(PIN_CHB_I_VRF)); + Serial.print(","); + delay(50); + Serial.print(analogRead(PIN_CHA_I_VRF)); + Serial.print(" "); + Serial.print(analogRead(PIN_CHB_I_VRF)); + Serial.println(""); + + cnt += 1; + if( cnt > 1 ) + { + cnt = 0; + step_val += 1; + if( step_val == 256 ) { + step_val = 0; + } + } + digitalWrite(PIN_LED, (step_val & 1) ? HIGH : LOW); + delay(100); +} + diff --git a/Arduino/test_r2_analog/test_r2_analog.ino b/Arduino/test_r2_analog/test_r2_analog.ino new file mode 100644 index 0000000000000000000000000000000000000000..9ccd25d9f874df6c90782dbc10755e896913e5b5 --- /dev/null +++ b/Arduino/test_r2_analog/test_r2_analog.ino @@ -0,0 +1,50 @@ +/* + TEST: DAC with feedback. + */ + +const int PIN_CHA_O_VRF = 5; +const int PIN_CHB_O_VRF = 6; +const int PIN_CHA_I_VRF = 14; //A2; +const int PIN_CHB_I_VRF = 15; //A5 +const int PIN_LED = 13; // Orange LED + +void setup() +{ + Serial.begin(9600); + pinMode(PIN_CHA_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHB_O_VRF, OUTPUT); // PWM + pinMode(PIN_LED, OUTPUT); +} + +int step_val; +int cnt; +void loop() +{ + Serial.print(step_val); + Serial.print(" "); + analogWrite(PIN_CHB_O_VRF, step_val); + analogWrite(PIN_CHA_O_VRF, step_val); + delay(50); + Serial.print(analogRead(PIN_CHA_I_VRF)); + Serial.print(" "); + Serial.print(analogRead(PIN_CHB_I_VRF)); + Serial.print(","); + delay(50); + Serial.print(analogRead(PIN_CHA_I_VRF)); + Serial.print(" "); + Serial.print(analogRead(PIN_CHB_I_VRF)); + Serial.println(""); + + cnt += 1; + if( cnt > 1 ) + { + cnt = 0; + step_val += 1; + if( step_val == 256 ) { + step_val = 0; + } + } + digitalWrite(PIN_LED, (step_val & 1) ? HIGH : LOW); + delay(100); +} + diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino new file mode 100644 index 0000000000000000000000000000000000000000..03be2fe684d284b59b722a863b4c31aa4fea8294 --- /dev/null +++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino @@ -0,0 +1,178 @@ +/* + ForceFeedback Test: Constant force movement. + BOARD: Rev2 + */ + +// To move with constant force, need: +// - A force (set with VREF) +// - A direction +// Always want the energised coils to be pulling one way +// - When it gets too close to the target position, swap upwards +// - When too far way, swap closer. +// NOTE: Want to track the coil phase (position and output) internally, so that if there's slip +// the sign can be recovered +// - Encoder `count/10` gives the step index, there's four phases to the steps (A+, B+, A-, B-) +// - Stepper will switch through these too. +// - To maintain constant force sign, we want the stepper's internal phase to be ahead/behind the position phase. + +// Step 1: Enable motor to position hold at 50% current, wait 100ms +// Step 2: Zero the encoder +// Step 3: Step once in target direction +// Loop: +// - If + +// NOTE: +// - Encoder is 1000 pulse (4000 counts) per revolution +// - Stepper is 400 steps per resolution +// > So, expect 10 counts (2.5 cycles) per stepper step. +// > Step the motor when a change of more than 10 is seen? + +// NOTE: DIR=0 is positive encoder (on CH1) + +const int INPUT_ENC1A = 2; +const int INPUT_ENC1B = 3; +const int OUTPUT_VREF1 = 5; // D5 +const int 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; + +int last_1, val_1; + +int ctrl_phase_1; // Current "phase" of the controller (0-3) +int axle_phase_1; // Current "phase" of the axle (0-3) + +// Addition modulo 4 +static int phase_add(int v, int i) { + v += i; + while(v > 3) v -= 4; + while(v < 0) v += 4; + return v; +} + +// Step channel once in the given direction +void step_once_1(int dir) { + ctrl_phase_1 = phase_add(ctrl_phase_1, dir < 0 ? -1 : 1); + + digitalWrite(OUTPUT_DIR1, dir < 0); + digitalWrite(OUTPUT_CLK1, 1); + delay(1); + digitalWrite(OUTPUT_CLK1, 0); +} + +void setup() +{ + Serial.begin(9600); + pinMode(OUTPUT_VREF1, OUTPUT); + pinMode(OUTPUT_MOT_ON, OUTPUT); + pinMode(OUTPUT_DIR1, OUTPUT); + pinMode(OUTPUT_CLK1, OUTPUT); +// pinMode(PIN_LED, OUTPUT); + + attachInterrupt(0, isr_enc1, CHANGE); + attachInterrupt(1, isr_enc1, CHANGE); + + // Set output current to 50% + analogWrite(OUTPUT_VREF1, 128); + analogWrite(OUTPUT_VREF2, 0); + delay(5); + // Read it back + Serial.print("VRef1 = "); + Serial.print(analogRead(INPUT_VREF1)); + Serial.println(""); + // Turn controllers on + digitalWrite(OUTPUT_MOT_ON, 1); + + // Wait some time + delay(100); + // Reset the counters + val_1 = 0; +} + +void loop() +{ + // 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 ) { + val_1 += 10; + axle_phase_1 = phase_add(axle_phase_1, -1); + } + else if( v >= 5 ) { + val_1 -= 10; + axle_phase_1 = phase_add(axle_phase_1, +1); + } + else { + // In-range + } + + // Compare `ctrl_phase_1` and `real_phase_1` to determine the current "mode" + // - Equal: The system is now holding, we want to push `ctrl` ahead/behind `real` (depending on target direction) + // - +/- One: Either pulling the wrong way (need to to step twice), or pulling the right way (no change) + // - 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 phase_diff = phase_add(ctrl_phase_1, target_ctrl_phase_1); + if( phase_diff == 0 ) { + // No need to change + } + else if( phase_diff == 3 ) { + // Need to shift phase up + step_once_1(1); + step_count += 1; + } + else if( phase_diff == 1 ) { + // Need to shift phase down + step_once_1(-1); + step_count -= 1; + } + else { + // Going opposite! - Run twice + step_once_1(1); + step_once_1(1); + err_count += 1; + step_count += 2; + } + + // TODO: Keep track of state to be printed (and only print if there's buffer space) + // - Current phase values + // - Total number of steps taken since last log + // - Number of times the phases have ended up opposed + if( false ) { + if( Serial.availableForWrite() > 20 ) { + Serial.print(v); Serial.print(" "); Serial.print(val_1); + Serial.print(" "); + Serial.println("STEP"); + last_val1 = val_1; + step_count = 0; + err_count = 0; + } + } +} + +const int E = -99; +const int GREY_MAPPING[16] = { + 0, -1, 1, E, + 1, 0, E, -1, + -1, E, 0, 1, + E, 1, -1, 0, +}; + +void isr_enc1() { + int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B); + if( v != last_1 ) { + int adj = GREY_MAPPING[last_1 * 4 + v]; + if(adj == E) { + } + else { + val_1 += adj; + } + last_1 = v; + } +} + diff --git a/Arduino/test_r2_encoder/test_r2_encoder.ino b/Arduino/test_r2_encoder/test_r2_encoder.ino new file mode 100644 index 0000000000000000000000000000000000000000..b4617c8b5c08f243659f51263fbf4247dc96e9e1 --- /dev/null +++ b/Arduino/test_r2_encoder/test_r2_encoder.ino @@ -0,0 +1,92 @@ +/* + ForceFeedback Test: Encoder counter + BOARD: Rev2 + */ +// #include "../board_v2.h" +const int INPUT_ENC1A = 2; +const int INPUT_ENC1B = 3; +const int INPUT_ENC2A = 4; +const int OUTPUT_VREF1 = 5; // D5 +const int OUTPUT_VREF2 = 6; // D6 +const int INPUT_ENC2B = 7; +const int OUTPUT_MOT_ON = 9; +const int OUTPUT_DIR2 = 10; +const int OUTPUT_CLK2 = 11; +const int OUTPUT_DIR1 = 12; +const int OUTPUT_CLK1 = 13; +const int INPUT_VREF2 = 14; // A0 +const int INPUT_VREF1 = 15; // A1 +const int INPUT_POT1 = 18; +const int INPUT_POT2 = 19; +const int PIN_LED = 13; // Orange LED + +void setup() +{ + Serial.begin(9600); + pinMode(OUTPUT_VREF1, OUTPUT); + pinMode(OUTPUT_VREF2, OUTPUT); + pinMode(OUTPUT_MOT_ON, OUTPUT); + pinMode(OUTPUT_DIR2, OUTPUT); + pinMode(OUTPUT_CLK2, OUTPUT); + pinMode(OUTPUT_DIR1, OUTPUT); + pinMode(OUTPUT_CLK1, OUTPUT); + pinMode(PIN_LED, OUTPUT); + + attachInterrupt(0, isr_enc1, CHANGE); + attachInterrupt(1, isr_enc1, CHANGE); +} + +int v; +int last_1, val_1; +int last_2, val_2; +const int E = -99; +const int GREY_MAPPING[16] = { + 0, -1, 1, E, + 1, 0, E, -1, + -1, E, 0, 1, + E, 1, -1, 0, +}; + +int err_count_1; +void isr_enc1() { + int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B); + if( v != last_1 ) { + // Count on rising edge of [A], depending on sign of [B] + int adj = GREY_MAPPING[last_1 * 4 + v]; + if(adj == E) { + err_count_1 += 1; + } + else { + val_1 += adj; + } + last_1 = v; + } +} +int last_seen_1; +void loop() +{ + if( val_1 != last_seen_1 ) { + Serial.print("Encoder 1: "); + int v = val_1; + Serial.print(last_seen_1); + Serial.print("->"); + Serial.print(v); + Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")"); + Serial.println(""); + last_seen_1 = v; + } + + v = digitalRead(INPUT_ENC2A) * 2 + digitalRead(INPUT_ENC2B); + if( v != last_2 ) { + int diff = v ^ last_2; + last_2 = v; + switch(diff) + { + case 0: break; + case 1: val_2 += 1; break; + case 2: val_2 -= 1; break; + case 3: break; + } + Serial.print("Encoder 2: "); Serial.println(val_2); + } +} diff --git a/Arduino/test_r2_stepper/test_r2_stepper.ino b/Arduino/test_r2_stepper/test_r2_stepper.ino new file mode 100644 index 0000000000000000000000000000000000000000..9c520757528af85e91db53699acd1030f9e3002d --- /dev/null +++ b/Arduino/test_r2_stepper/test_r2_stepper.ino @@ -0,0 +1,93 @@ +/* + ForceFeedback Test: Stepper moving with encoder + BOARD: Rev2 + */ +// #include "../board_v2.h" +const int INPUT_ENC1A = 2; +const int INPUT_ENC1B = 3; +const int INPUT_ENC2A = 4; +const int OUTPUT_VREF1 = 5; // D5 +const int OUTPUT_VREF2 = 6; // D6 +const int INPUT_ENC2B = 7; +const int OUTPUT_MOT_ON = 9; +const int OUTPUT_DIR2 = 10; +const int OUTPUT_CLK2 = 11; +const int OUTPUT_DIR1 = 12; +const int OUTPUT_CLK1 = 13; +const int INPUT_VREF2 = 14; // A0 +const int INPUT_VREF1 = 15; // A1 +const int INPUT_POT1 = 18; +const int INPUT_POT2 = 19; +const int PIN_LED = 13; // Orange LED + + +const int E = -99; +const int GREY_MAPPING[16] = { + 0, -1, 1, E, + 1, 0, E, -1, + -1, E, 0, 1, + E, 1, -1, 0, +}; +int last_1, val_1; +int err_count_1; + +void setup() +{ + Serial.begin(9600); + pinMode(OUTPUT_VREF1, OUTPUT); + pinMode(OUTPUT_VREF2, OUTPUT); + pinMode(OUTPUT_MOT_ON, OUTPUT); + pinMode(OUTPUT_DIR2, OUTPUT); + pinMode(OUTPUT_CLK2, OUTPUT); + pinMode(OUTPUT_DIR1, OUTPUT); + pinMode(OUTPUT_CLK1, OUTPUT); + pinMode(PIN_LED, OUTPUT); + + attachInterrupt(0, isr_enc1, CHANGE); + attachInterrupt(1, isr_enc1, CHANGE); + + digitalWrite(OUTPUT_MOT_ON, 1); + digitalWrite(OUTPUT_DIR1, 0); + delay(100); + val_1 = 0; +} + +void isr_enc1() { + int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B); + if( v != last_1 ) { + // Count on rising edge of [A], depending on sign of [B] + int adj = GREY_MAPPING[last_1 * 4 + v]; + if(adj == E) { + err_count_1 += 1; + } + else { + val_1 += adj; + } + last_1 = v; + } +} +int last_seen_1; +int delay_count; +void loop() +{ + if( val_1 != last_seen_1 ) { + Serial.print("Encoder 1: "); + int v = val_1; + Serial.print(last_seen_1); + Serial.print("->"); + Serial.print(v); + Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")"); + Serial.println(""); + last_seen_1 = v; + } + + // Shift the motor every 1s + if( delay_count > 100 ) { + delay_count = 0; + digitalWrite(OUTPUT_CLK1, 1); + delay(1); + digitalWrite(OUTPUT_CLK1, 0); + } + delay(10); + delay_count += 1; +} diff --git a/Arduino/test_stepper/test_stepper.ino b/Arduino/test_stepper/test_stepper.ino new file mode 100644 index 0000000000000000000000000000000000000000..e040736c65a104a34a3ab8a49dad63be143f7c3a --- /dev/null +++ b/Arduino/test_stepper/test_stepper.ino @@ -0,0 +1,41 @@ +/* + ForceFeedback Test: Stepper movement + */ +const int PIN_CHA_O_DIR = 2; //D2; +const int PIN_CHA_O_VRF = 3; //D3; +const int PIN_CHA_O_CLK = 4; //D4; +const int PIN_CHB_O_DIR = 5; //D5; +const int PIN_CHB_O_VRF = 6; //D6; +const int PIN_CHB_O_CLK = 7; //D7; +const int PIN_CHA_I_POT = 14; //A0; +const int PIN_CHA_I_ENC = 15; //A1; +const int PIN_CHA_I_VRF = 16; //A2; +const int PIN_CHB_I_POT = 17; //A3; +const int PIN_CHB_I_ENC = 18; //A4; +const int PIN_CHB_I_VRF = 19; //A5; +const int PIN_LED = 13; // Orange LED + +void setup() +{ + Serial.begin(9600); + pinMode(PIN_CHA_O_DIR, OUTPUT); + pinMode(PIN_CHA_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHA_O_CLK, OUTPUT); + pinMode(PIN_CHB_O_DIR, OUTPUT); + pinMode(PIN_CHB_O_VRF, OUTPUT); // PWM + pinMode(PIN_CHB_O_CLK, OUTPUT); + pinMode(PIN_LED, OUTPUT); + + digitalWrite(PIN_CHA_O_DIR, LOW); +} + + +void loop() +{ + digitalWrite(PIN_CHA_O_CLK, HIGH); + digitalWrite(PIN_LED, HIGH); + delay(100); + digitalWrite(PIN_CHA_O_CLK, LOW); + digitalWrite(PIN_LED, LOW); + delay(100); +}