![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
robot arm
Dependencies: mbed BufferedSerial
main.cpp@0:07a5cc567526, 2021-03-27 (annotated)
- Committer:
- howanglam3
- Date:
- Sat Mar 27 07:53:20 2021 +0000
- Revision:
- 0:07a5cc567526
arm move;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
howanglam3 | 0:07a5cc567526 | 1 | #include "mbed.h" |
howanglam3 | 0:07a5cc567526 | 2 | #include "BufferedSerial.h" |
howanglam3 | 0:07a5cc567526 | 3 | |
howanglam3 | 0:07a5cc567526 | 4 | |
howanglam3 | 0:07a5cc567526 | 5 | #define GET_LOW_char(A) (uint8_t)((A)) |
howanglam3 | 0:07a5cc567526 | 6 | //Macro function get lower 8 bits of A |
howanglam3 | 0:07a5cc567526 | 7 | #define GET_HIGH_char(A) (uint8_t)((A) >> 8) |
howanglam3 | 0:07a5cc567526 | 8 | //Macro function get higher 8 bits of A |
howanglam3 | 0:07a5cc567526 | 9 | #define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B)) |
howanglam3 | 0:07a5cc567526 | 10 | //put A as higher 8 bits B as lower 8 bits which amalgamated into 16 bits integer |
howanglam3 | 0:07a5cc567526 | 11 | #define ID1 1 |
howanglam3 | 0:07a5cc567526 | 12 | #define ID2 2 |
howanglam3 | 0:07a5cc567526 | 13 | #define ID3 3 |
howanglam3 | 0:07a5cc567526 | 14 | #define ID4 4 |
howanglam3 | 0:07a5cc567526 | 15 | |
howanglam3 | 0:07a5cc567526 | 16 | #define LOBOT_SERVO_FRAME_HEADER 0x55 |
howanglam3 | 0:07a5cc567526 | 17 | #define LOBOT_SERVO_MOVE_TIME_WRITE 1 |
howanglam3 | 0:07a5cc567526 | 18 | |
howanglam3 | 0:07a5cc567526 | 19 | using namespace std; |
howanglam3 | 0:07a5cc567526 | 20 | |
howanglam3 | 0:07a5cc567526 | 21 | BufferedSerial serial_port(PA_9, PA_10, 16, 16); // tx, rx, buffer size, multiple |
howanglam3 | 0:07a5cc567526 | 22 | |
howanglam3 | 0:07a5cc567526 | 23 | DigitalOut myled(LED1); |
howanglam3 | 0:07a5cc567526 | 24 | |
howanglam3 | 0:07a5cc567526 | 25 | char LobotCheckSum(char buf[]) |
howanglam3 | 0:07a5cc567526 | 26 | { |
howanglam3 | 0:07a5cc567526 | 27 | char i; |
howanglam3 | 0:07a5cc567526 | 28 | uint16_t temp = 0; |
howanglam3 | 0:07a5cc567526 | 29 | for (i = 2; i < buf[3] + 2; i++) { |
howanglam3 | 0:07a5cc567526 | 30 | temp += buf[i]; |
howanglam3 | 0:07a5cc567526 | 31 | } |
howanglam3 | 0:07a5cc567526 | 32 | temp = ~temp; |
howanglam3 | 0:07a5cc567526 | 33 | i = (char)temp; |
howanglam3 | 0:07a5cc567526 | 34 | return i; |
howanglam3 | 0:07a5cc567526 | 35 | } |
howanglam3 | 0:07a5cc567526 | 36 | |
howanglam3 | 0:07a5cc567526 | 37 | |
howanglam3 | 0:07a5cc567526 | 38 | void LobotSerialoneServoMove(uint8_t id, int16_t position, uint16_t time) |
howanglam3 | 0:07a5cc567526 | 39 | { |
howanglam3 | 0:07a5cc567526 | 40 | char buf[10]; |
howanglam3 | 0:07a5cc567526 | 41 | if(position < 0) |
howanglam3 | 0:07a5cc567526 | 42 | position = 0; |
howanglam3 | 0:07a5cc567526 | 43 | if(position > 1000) |
howanglam3 | 0:07a5cc567526 | 44 | position = 1000; |
howanglam3 | 0:07a5cc567526 | 45 | buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER; |
howanglam3 | 0:07a5cc567526 | 46 | buf[2] = 0x08; |
howanglam3 | 0:07a5cc567526 | 47 | buf[3] = 0x03; |
howanglam3 | 0:07a5cc567526 | 48 | buf[4] = 0x01; |
howanglam3 | 0:07a5cc567526 | 49 | buf[5] = GET_LOW_char(time); |
howanglam3 | 0:07a5cc567526 | 50 | buf[6] = GET_HIGH_char(time); |
howanglam3 | 0:07a5cc567526 | 51 | buf[7] = id; |
howanglam3 | 0:07a5cc567526 | 52 | buf[8] = GET_LOW_char(position); |
howanglam3 | 0:07a5cc567526 | 53 | buf[9] = GET_HIGH_char(position); |
howanglam3 | 0:07a5cc567526 | 54 | serial_port.write(buf, 10); |
howanglam3 | 0:07a5cc567526 | 55 | } |
howanglam3 | 0:07a5cc567526 | 56 | |
howanglam3 | 0:07a5cc567526 | 57 | |
howanglam3 | 0:07a5cc567526 | 58 | int main() { |
howanglam3 | 0:07a5cc567526 | 59 | //serial_port.set_baud(115200); |
howanglam3 | 0:07a5cc567526 | 60 | //serial_port.set_format(8,BufferedSerial::None,1); |
howanglam3 | 0:07a5cc567526 | 61 | //char buf[10]={0}; |
howanglam3 | 0:07a5cc567526 | 62 | while(1){ |
howanglam3 | 0:07a5cc567526 | 63 | myled = 1; |
howanglam3 | 0:07a5cc567526 | 64 | LobotSerialoneServoMove(ID1, 700, 1000);//id, pos, time |
howanglam3 | 0:07a5cc567526 | 65 | LobotSerialoneServoMove(ID2, 500, 1000); |
howanglam3 | 0:07a5cc567526 | 66 | wait_ms(1000); |
howanglam3 | 0:07a5cc567526 | 67 | LobotSerialoneServoMove(ID1, 500, 1000); |
howanglam3 | 0:07a5cc567526 | 68 | LobotSerialoneServoMove(ID2, 600, 1000); |
howanglam3 | 0:07a5cc567526 | 69 | wait_ms(1000); |
howanglam3 | 0:07a5cc567526 | 70 | LobotSerialoneServoMove(ID1, 900, 1000); |
howanglam3 | 0:07a5cc567526 | 71 | LobotSerialoneServoMove(ID2, 700, 1000); |
howanglam3 | 0:07a5cc567526 | 72 | wait_ms(1000); |
howanglam3 | 0:07a5cc567526 | 73 | LobotSerialoneServoMove(ID1, 500, 1000); |
howanglam3 | 0:07a5cc567526 | 74 | LobotSerialoneServoMove(ID2, 600, 1000); |
howanglam3 | 0:07a5cc567526 | 75 | wait_ms(1000); |
howanglam3 | 0:07a5cc567526 | 76 | myled = 0; |
howanglam3 | 0:07a5cc567526 | 77 | wait_ms(1000); |
howanglam3 | 0:07a5cc567526 | 78 | } |
howanglam3 | 0:07a5cc567526 | 79 | } |