Diff: main.cpp

Revision:

0:dcdfdfd1f10c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 17 21:19:48 2022 +0000
@@ -0,0 +1,133 @@
+#include "mbed.h" 
+
+Serial async_port(p9, p10); 
+
+char command [10]; // command 3 aarray
+char command_stop [4]; // command 6 array 
+
+// constant header value
+#define SERVO_FRAME_HEADER  0x55
+
+// Servo ID
+#define ID_1 0x01
+#define ID_2 0x02
+#define ID_3 0x03
+#define ID_4 0x04
+#define ID_5 0x05
+#define ID_6 0x06
+
+#define GET_LOW_BYTE(A) (uint8_t)((A))
+//Macro function  get lower 8 bits of A
+#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
+//Macro function  get higher 8 bits of A
+
+void moveOneServo(char ID,int16_t position){ 
+// move any servo at a time, parameters (ID_# , position 0-1000)
+ 
+    if(position < 0)
+    position = 0;
+    if(position > 1000)
+    position = 1000;
+    
+    command [0] = command [1] = SERVO_FRAME_HEADER; 
+    command [2] = 0x08;   // length
+    command [3] = 0x03;   // Command
+    command [4] = 0x01;   // # of servos
+    command [5] = 0xD8;   // Ltime
+    command [6] = 0x07;   // Htime
+    command [7] = ID;     // ID
+    command [8] = GET_LOW_BYTE(position);
+    command [9] = GET_HIGH_BYTE(position);
+    
+    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
+    
+/*/ notes: 
+    CMD_SERVO_MOVE ( command 3) 
+*/
+}
+
+int main() {
+    
+// note: wait should be 5s to allow the servo enough time to position itself
+
+    async_port.baud(9600); // boud rate provided by user manual
+
+    while(1) { 
+    
+        moveOneServo(ID_3,0); 
+        wait(5); 
+        moveOneServo(ID_3,1000);
+        wait(5);
+        
+//Testing
+        ////////////////////////
+/*
+        closeGrabber();
+        wait(1); 
+        //stopProgram();
+        //wait(1); 
+        openGrabber();
+        //wait(1); 
+        //stopProgram();
+        wait(1);     
+        ////////////////////////
+        for(int i=0; i<10; i++){async_port.putc(close_grabber[i]);} 
+        wait(1); 
+        for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
+        wait(1); 
+        for(int i=0; i<10; i++){async_port.putc(open_grabber[i]);}
+        wait(1); 
+        for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
+        wait(1); 
+*/
+    } 
+
+} 
+void closeGrabber(){ // function to only close grabber
+    
+    command [0] = command [1] = SERVO_FRAME_HEADER; 
+    command [2] = 0x08;
+    command [3] = 0x03;
+    command [4] = 0x01;
+    command [5] = 0xE8;
+    command [6] = 0x03;
+    command [7] = 0x01;
+    command [8] = 0xD8;
+    command [9] = 0x07;
+    
+    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
+    
+/*/ notes: 
+    CMD_SERVO_MOVE ( command 3) 
+*/
+}
+void openGrabber(){// function to only open grabber
+    
+    command [0] = command [1] = SERVO_FRAME_HEADER; 
+    command [2] = 0x08;
+    command [3] = 0x03;
+    command [4] = 0x01;
+    command [5] = 0xE8;
+    command [6] = 0x03;
+    command [7] = 0x01;
+    command [8] = 0x00;
+    command [9] = 0x00;
+    
+    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
+/*/ notes: 
+    CMD_SERVO_MOVE ( command 3) 
+*/
+}
+void stopProgram(){// function to stop all programs
+    
+    command [0] = command [1] = SERVO_FRAME_HEADER; 
+    command [2] = 0x02;
+    command [3] = 0x07;
+    
+    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
+    
+/*/ notes: 
+    CMD_ACTION_STOP( command 6) 
+*/
+    
+}
\ No newline at end of file