Skip to content
Snippets Groups Projects
Commit 37e19776 authored by John Hodge's avatar John Hodge
Browse files

Arduino - Testing arduino code (including draft ConstantForce)

parent f11908ee
Branches
No related merge requests found
/*
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);
}
/*
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);
}
/*
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);
}
/*
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;
}
}
/*
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);
}
}
/*
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;
}
/*
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);
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment