Diff: Serialservo.cpp

Revision:

0:2d468b5749f9

Child:

1:4caf52a715c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serialservo.cpp	Sun Mar 18 06:38:05 2018 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+#include "Serialservo.h"
+
+long Serialservo::map(long x, long in_min, long in_max, long out_min, long out_max){
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+Serialservo::Serialservo(PinName TX, PinName RX) : ser(TX, RX){
+}
+
+void Serialservo::init(char id){
+    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);
+    unsigned char h,l,head;
+    int temp1, temp2;
+    temp1 = (value & 0x3F80) >> 7;
+    temp2 = value & 0x7f;
+    head = 0x80 | servoval;
+    h = (unsigned char)temp1;
+    l = (unsigned char)temp2;
+    ser.putc(head);
+    ser.putc(h);
+    ser.putc(l);
+}
+    
\ No newline at end of file