diff options
| -rw-r--r-- | arduino_sketch/rfc4824/rfc4824.ino | 110 |
1 files changed, 10 insertions, 100 deletions
diff --git a/arduino_sketch/rfc4824/rfc4824.ino b/arduino_sketch/rfc4824/rfc4824.ino index 9a5e2f6..00e8e18 100644 --- a/arduino_sketch/rfc4824/rfc4824.ino +++ b/arduino_sketch/rfc4824/rfc4824.ino @@ -12,6 +12,11 @@ Stepper right_arm(64,8,9,10,11); int c; +int pos[16][2] = { {1,0}, {2,0}, {3,0}, {4,0}, + {0,3}, {0,2}, {0,1}, {2,-1}, + {3,-1}, {4,2}, {1,4}, {1,3}, + {1,2}, {1,1}, {2,5}, {2,4} }; + void setup() { Serial.begin(9600); left_arm.setSpeed(QRT_SPEED); @@ -100,106 +105,11 @@ void read_serial_commands() { void set_hex(char digit) { Serial.print("Going to position "); Serial.println(digit,HEX); - switch (digit) { - case 0x0: - right_arm.step(QRT_STP); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP); - break; - case 0x1: - right_arm.step(QRT_STP*2); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*2); - break; - case 0x2: - right_arm.step(QRT_STP*3); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*3); - break; - case 0x3: - right_arm.step(QRT_STP*4); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*4); - break; - case 0x4: - left_arm.step(-QRT_STP*3); - delay(CHAR_DELAY); - left_arm.step(QRT_STP*3); - break; - case 0x5: - left_arm.step(-QRT_STP*2); - delay(CHAR_DELAY); - left_arm.step(QRT_STP*2); - break; - case 0x6: - left_arm.step(-QRT_STP); - delay(CHAR_DELAY); - left_arm.step(QRT_STP); - break; - case 0x7: - right_arm.step(QRT_STP*2); - left_arm.step(QRT_STP); - delay(CHAR_DELAY); - left_arm.step(-QRT_STP); - right_arm.step(-QRT_STP*2); - break; - case 0x8: - right_arm.step(QRT_STP*3); - left_arm.step(QRT_STP); - delay(CHAR_DELAY); - left_arm.step(-QRT_STP); - right_arm.step(-QRT_STP*3); - break; - case 0x9: - right_arm.step(QRT_STP*4); - left_arm.step(-QRT_STP*2); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*4); - left_arm.step(QRT_STP*2); - break; - case 0xA: - right_arm.step(QRT_STP); - left_arm.step(-QRT_STP*4); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP); - left_arm.step(QRT_STP*4); - break; - case 0xB: - right_arm.step(QRT_STP); - left_arm.step(-QRT_STP*3); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP); - left_arm.step(QRT_STP*3); - break; - case 0xC: - right_arm.step(QRT_STP); - left_arm.step(-QRT_STP*2); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP); - left_arm.step(QRT_STP*2); - break; - case 0xD: - right_arm.step(QRT_STP); - left_arm.step(-QRT_STP); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP); - left_arm.step(QRT_STP); - break; - case 0xE: - right_arm.step(QRT_STP*2); - left_arm.step(-QRT_STP*5); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*2); - left_arm.step(QRT_STP*5); - break; - case 0xF: - right_arm.step(QRT_STP*2); - left_arm.step(-QRT_STP*4); - delay(CHAR_DELAY); - right_arm.step(-QRT_STP*2); - left_arm.step(QRT_STP*4); - break; - } + right_arm.step(QRT_STP*pos[digit][0]); + left_arm.step(-1*QRT_STP*pos[digit][1]); + delay(CHAR_DELAY); + left_arm.step(-1*QRT_STP*pos[digit][1]); + right_arm.step(QRT_STP*pos[digit][0]); delay(CHAR_DELAY); Serial.println("Returned to home position"); } |