Update and rename armdoid.cpp to armdoid.hpp

Author: dragoncoder047

code/armdroid-class/armdoid.cpp renamed to code/armdroid-class/armdoid.hpp

@@ -12,34 +12,35 @@ const uint8_t PHASETAB[8] = {
 };
 
 // use an enum here ??
-#define BASE 0
-#define SHOULDER 1
-#define ELBOW 2
-#define WRIST_LEFT 3
-#define WRIST_RIGHT 4
-#define GRIPPER 5
-#define ACCESSORY 6
-#define JACKS 7
+#define BASE 6
+#define SHOULDER 5
+#define ELBOW 4
+#define WRIST_LEFT 2
+#define WRIST_RIGHT 3
+#define GRIPPER 1
+#define ACCESSORY 7
+#define JACKS 8
 
 class Armdroid {
     private:
         uint8_t port[8];
-        long current_positions[8];
-        long target_positions[8];
-        long phases[8];
+        int64_t current_positions[8];
+        int64_t target_positions[8];
+        uint8_t phases[8];
         uint8_t dividers[8];
         uint8_t counters[8];
     public:
     Armdroid(uint8_t a, uint8_t b, uint8_t c, uint8_t d, uint8_t x, uint8_t y, uint8_t z, uint8_t k) {
-        this->port[0] = a;
-        this->port[1] = b;
-        this->port[2] = c;
-        this->port[3] = d;
-        this->port[4] = x;
-        this->port[5] = y;
-        this->port[6] = z;
+        this->port[0] = x;
+        this->port[1] = y;
+        this->port[2] = z;
+        this->port[3] = a;
+        this->port[4] = b;
+        this->port[5] = c;
+        this->port[6] = d;
         this->port[7] = k;
         for (uint8_t i = 0; i < 8; i++) pinMode(this->port[i], OUTPUT);
+        digitalWrite(this->port[7], HIGH);
     }
     void motorGoto(uint8_t m, long pos) {
         this->target_positions[m] = pos;
@@ -84,15 +85,15 @@ class Armdroid {
         }
     }
     void writeToPort(uint8_t address, uint8_t data) {
-        uint8_t seven = data << 3 | address;
+        uint8_t bits = data << 3 | address;
         for (uint8_t i = 0; i < 7; i++) {
-            digitalWrite(this->port[i], seven & (128 >> i) ? HIGH : LOW);
+            digitalWrite(this->port[i], bits & (1 << i) ? HIGH : LOW);
         }
-        delayMicroseconds(10);
-        digitalWrite(this->port[7], HIGH);
-        delayMicroseconds(10);
+        delayMicroseconds(20);
         digitalWrite(this->port[7], LOW);
-        delayMicroseconds(10);
+        delayMicroseconds(20);
+        digitalWrite(this->port[7], HIGH);
+        delayMicroseconds(20);
     }
     void torqueOff() {
         for (uint8_t i = 0; i < 8; i++) {