Diff: arm.cpp

Revision:

0:ed8d2fd5fb9d

diff -r 000000000000 -r ed8d2fd5fb9d arm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arm.cpp	Sun Nov 27 04:35:20 2022 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "arm.h"
+
+
+//note: can be changed but we should leave alone 
+
+//Macro function  get lower 8 bits of A
+//#define GET_LOW_BYTE(A) (uint8_t)((A))
+//Macro function  get higher 8 bits of A
+//#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
+
+#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
+#define GET_LOW_BYTE(A) (uint8_t)((A))
+
+
+
+//initialize serial ports
+Arm::Arm(PinName tx, PinName rx) : async_port(tx,rx){
+    //async_port(tx, rx);
+}
+
+void Arm::moveServo(int16_t motor, int16_t position, int16_t time){ 
+// main function to move any servo at a time, parameters (ID_# (1-6) , position 0-1000)
+    char command[10];
+    
+    //get motor id
+    char id = getID(motor);
+    
+    command [0] = command [1] = 0x55; //start of communication 
+    command [2] = 0x08;   // length = 8 to control a single servo at a time
+    command [3] = 0x03;   // Command: CMD_SERVO_MOVE
+    command [4] = id;   // servo #
+    command [5] = GET_LOW_BYTE(time);
+    command [6] = GET_HIGH_BYTE(time);
+    command [7] = id;     // servo #
+    command [8] = GET_LOW_BYTE(position);
+    command [9] = GET_HIGH_BYTE(position);
+    
+    // send information to the servo
+    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
+    
+/*/ notes: 
+    CMD_SERVO_MOVE ( command 3)
+    Command [2] or length is equal to N+2 where N is the number of parameters in bytes
+    that are sent to the servo. These 'N' parameters include command[4] to command[9].
+*/
+}
+
+ char Arm::getID(int16_t motor){
+    char id;
+    
+    switch(motor){
+        case 1:
+            id = 0x01;
+            break;
+        case 2:
+            id = 0x02;
+            break;
+        case 3:
+            id = 0x03;
+            break;
+        case 4:
+            id = 0x04;
+            break;
+        case 5:
+            id = 0x05;
+            break;
+        case 6:
+            id = 0x06;
+            break;
+    }
+    return id;
+}