ケンタ ミヤザキ / Serialservo

Dependents:   Serialservo_01 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more

Serialservo.cpp

Committer:
kenken0721
Date:
2018-03-18
Revision:
0:2d468b5749f9
Child:
1:4caf52a715c2

File content as of revision 0:2d468b5749f9:

#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);
}