Diff: Serialservo.cpp

Revision:

1:4caf52a715c2

Parent:

0:2d468b5749f9

Child:

2:da6abd6afe26

--- a/Serialservo.cpp	Sun Mar 18 06:38:05 2018 +0000
+++ b/Serialservo.cpp	Mon Nov 05 09:09:16 2018 +0000
@@ -8,24 +8,48 @@
 Serialservo::Serialservo(PinName TX, PinName RX) : ser(TX, RX){
 }
 
-void Serialservo::init(char id){
+void Serialservo::init(){
     ser.baud(115200);
     ser.format(8, Serial::Even, 1);
-    servoval = id;
 }
 
-void Serialservo::move(int value){
-    //value += 80;
-    value = (int)map((int)value, 270, 0, 3500, 11500);
+
+void Serialservo::stretch(char id,int value){
     unsigned char h,l,head;
     int temp1, temp2;
-    temp1 = (value & 0x3F80) >> 7;
-    temp2 = value & 0x7f;
-    head = 0x80 | servoval;
+    head = 0xC0 | id;
+    temp1 = 0x01;
+    temp2 = value;
     h = (unsigned char)temp1;
     l = (unsigned char)temp2;
     ser.putc(head);
     ser.putc(h);
     ser.putc(l);
 }
-    
\ No newline at end of file
+
+void Serialservo::speed(char id,int value){
+    unsigned char h,l,head;
+    int temp1, temp2;
+    head = 0xC0 | id;
+    temp1 = 0x02;
+    temp2 = value;
+    h = (unsigned char)temp1;
+    l = (unsigned char)temp2;
+    ser.putc(head);
+    ser.putc(h);
+    ser.putc(l);
+}
+
+void Serialservo::move(char id,int value){
+    value = (int)map((int)value, 270, 0, 3500, 11500);
+    unsigned char h,l,head;
+    int temp1, temp2;
+    temp1 = (value & 0x3F80) >> 7;
+    temp2 = value & 0x7f;
+    head = 0x80 | id;
+    h = (unsigned char)temp1;
+    l = (unsigned char)temp2;
+    ser.putc(head);
+    ser.putc(h);
+    ser.putc(l);
+}
\ No newline at end of file