update code that works

Author: dragoncoder047

code/armdroid-class/armbase.hpp

@@ -16,10 +16,10 @@ const uint8_t PHASETAB[8] = {
 #define SHOULDER 5
 #define ELBOW 4
 #define WRIST_LEFT 2
-#define WRIST_RIGHT 0
+#define WRIST_RIGHT 3
 #define GRIPPER 1
 #define ACCESSORY 7
-#define JACKS 3
+#define JACKS 0
 
 class AbstractArmdroid {
     private:
@@ -80,4 +80,4 @@ class AbstractArmdroid {
         }
     }
     virtual void writeToPort(uint8_t address, uint8_t data) {};
-};
+};

code/armdroid-class/armdroid-class.ino

@@ -1,14 +1,25 @@
+#define ARMDROID_DEBUG
 #include "armdroid.hpp"
 
+uint8_t bitReverse(uint8_t data, const uint8_t numBits) {
+    uint8_t out = 0;
+    for (uint8_t i = 0; i < numBits; i++) {
+        out = (out << 1) | (data & 1);
+        data >>= 1;
+    }
+    return out;
+}
+
 uint8_t pack(uint8_t address, uint8_t data) {
-    uint8_t revAddress = ((address & 4) >> 2) | (address & 2) | ((address & 1) << 2);
-    return revAddress << 4 | data;
+    uint8_t aa = address >> 1 | (address & 1) << 2;
+    uint8_t dd = data >> 2 | (data & 3) << 2;
+    return aa << 4 | dd;
 }
 
 PartialSerialArmdroid arm(8, 7, 3, pack);
 
 void setup() {
-  Serial.begin(115200);
+    Serial.begin(115200);
     pinMode(A0, INPUT);
     pinMode(A1, INPUT);
     pinMode(A2, INPUT);
@@ -20,15 +31,15 @@ void setup() {
 
 #define mmm(N, P) \
 r = analogRead(A##N); \
-if (abs(r - 512) > 50) { \
-    arm.motorMoveby(P, map(r, 1023, 0, -20, 20)); \
+if (abs(r - 512) > 20) { \
+    arm.motorMoveby(P, map(r, 0, 1023, -20, 20)); \
     t += abs(r - 512); \
 }
 
 void loop() {
     for (int i = 0; i < 20; i++) {
         arm.tick();
-        delayMicroseconds(300);
+        delayMicroseconds(700);
     }
     int r;
     unsigned long t = 0;
@@ -39,4 +50,4 @@ void loop() {
     mmm(4, GRIPPER);
     mmm(5, ELBOW);
     digitalWrite(13, t > 0);
-}
+}

code/armdroid-class/armdroid.hpp

@@ -54,6 +54,9 @@ class PartialSerialArmdroid : public AbstractArmdroid {
     void writeToPort(uint8_t address, uint8_t data) {
         uint8_t b = this->packBits(address, data);
         for (int i = 0; i < 7; i++) {
+            #ifdef ARMDROID_DEBUG
+            Serial.write('0' + (b & 1));
+            #endif
             digitalWrite(this->dataPin, b & 1);
             delayMicroseconds(30);
             digitalWrite(this->clockPin, HIGH);
@@ -62,8 +65,11 @@ class PartialSerialArmdroid : public AbstractArmdroid {
             delayMicroseconds(30);
             b >>= 1;
         }
+        #ifdef ARMDROID_DEBUG
+        Serial.println();
+        #endif
         digitalWrite(this->latchPin, HIGH);
-        delayMicroseconds(30);
+        delayMicroseconds(60);
         digitalWrite(this->latchPin, LOW);
     }
-};
+};