Panda House / Mbed 2 deprecated mbed_RS30X_Program_sample

双葉製コマンド方式サーボをシリアル通信にて動作させるプログラムです。 RS485通信方式　及び　TTL通信方式にて動作可能です。 接続方法については別途資料を準備中です。

Dependencies:   mbed

main.cpp@2:57157aa50af8, 2012-11-19 (annotated)

Committer:
PandaHouse
Date:
Mon Nov 19 09:00:51 2012 +0000
Revision:
2:57157aa50af8
Parent:
1:b9dfa2be76db 
Power supply injection comment addition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PandaHouse 1:b9dfa2be76db 1 /*-------------------------------------------------*/
PandaHouse 1:b9dfa2be76db 2 /* Turnof the power supplyinjection
PandaHouse 2:57157aa50af8 3 /* 1. Servo
PandaHouse 2:57157aa50af8 4 /* 2. mbed
PandaHouse 1:b9dfa2be76db 5 /*-------------------------------------------------*/
PandaHouse 1:b9dfa2be76db 6
PandaHouse 0:6d54a81f6e58 7 #include "mbed.h"
PandaHouse 0:6d54a81f6e58 8 Serial device(p9, p10); // tx = P9, rx = P10
PandaHouse 0:6d54a81f6e58 9 DigitalOut REDE(p11); // RS485 Transmit Enable
PandaHouse 0:6d54a81f6e58 10 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 11 /* Funcyion : mbed initialize */
PandaHouse 0:6d54a81f6e58 12 /* NAME : init */
PandaHouse 0:6d54a81f6e58 13 /* Argument : --- */
PandaHouse 0:6d54a81f6e58 14 /* Return value : --- */
PandaHouse 0:6d54a81f6e58 15 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 16 void init(void){
PandaHouse 0:6d54a81f6e58 17 device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
PandaHouse 0:6d54a81f6e58 18 REDE = 0; // RS485 Transmit disable
PandaHouse 0:6d54a81f6e58 19 }
PandaHouse 0:6d54a81f6e58 20 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 21 /* Funcyion : servo torque enable */
PandaHouse 0:6d54a81f6e58 22 /* NAME : torque */
PandaHouse 0:6d54a81f6e58 23 /* Argument : ID (Servo ID) */
PandaHouse 0:6d54a81f6e58 24 /* : data (Torque enable) */
PandaHouse 0:6d54a81f6e58 25 /* Return value : --- */
PandaHouse 0:6d54a81f6e58 26 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 27 void torque (unsigned char ID, unsigned char data){
PandaHouse 0:6d54a81f6e58 28
PandaHouse 0:6d54a81f6e58 29 unsigned char TxData[9]; // TransmitByteData [9byte]
PandaHouse 0:6d54a81f6e58 30 unsigned char CheckSum = 0; // CheckSum calculation
PandaHouse 0:6d54a81f6e58 31
PandaHouse 0:6d54a81f6e58 32 TxData[0] = 0xFA; // Header
PandaHouse 0:6d54a81f6e58 33 TxData[1] = 0xAF; // Header
PandaHouse 0:6d54a81f6e58 34 TxData[2] = ID; // ID
PandaHouse 0:6d54a81f6e58 35 TxData[3] = 0x00; // Flags
PandaHouse 0:6d54a81f6e58 36 TxData[4] = 0x24; // Address
PandaHouse 0:6d54a81f6e58 37 TxData[5] = 0x01; // Length
PandaHouse 0:6d54a81f6e58 38 TxData[6] = 0x01; // Count
PandaHouse 0:6d54a81f6e58 39 TxData[7] = data; // Data
PandaHouse 0:6d54a81f6e58 40
PandaHouse 0:6d54a81f6e58 41 // CheckSum calculation
PandaHouse 0:6d54a81f6e58 42 for(int i=2; i<=7; i++){
PandaHouse 0:6d54a81f6e58 43 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
PandaHouse 0:6d54a81f6e58 44 }
PandaHouse 0:6d54a81f6e58 45
PandaHouse 0:6d54a81f6e58 46 TxData[8] = CheckSum; // Sum
PandaHouse 0:6d54a81f6e58 47
PandaHouse 0:6d54a81f6e58 48 // Send Packet
PandaHouse 0:6d54a81f6e58 49 REDE = 1; // RS485 Transmit Enable
PandaHouse 0:6d54a81f6e58 50 for(int i=0; i<=8; i++){
PandaHouse 0:6d54a81f6e58 51 device.putc(TxData[i]);
PandaHouse 0:6d54a81f6e58 52 }
PandaHouse 0:6d54a81f6e58 53 wait_us(250); // Wait for transmission
PandaHouse 0:6d54a81f6e58 54 REDE = 0; // RS485 Transmitt disable
PandaHouse 0:6d54a81f6e58 55 }
PandaHouse 0:6d54a81f6e58 56 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 57 /* Funcyion : servo goal position instruction */
PandaHouse 0:6d54a81f6e58 58 /* NAME : GoalPosition */
PandaHouse 0:6d54a81f6e58 59 /* Argument : ID (Servo ID) */
PandaHouse 0:6d54a81f6e58 60 /* : data (Goal Position) */
PandaHouse 0:6d54a81f6e58 61 /* Return value : --- */
PandaHouse 0:6d54a81f6e58 62 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 63 void GoalPosition (unsigned char ID, int data){
PandaHouse 0:6d54a81f6e58 64
PandaHouse 0:6d54a81f6e58 65 unsigned char TxData[10]; // TransmitByteData [10byte]
PandaHouse 0:6d54a81f6e58 66 unsigned char CheckSum = 0; // CheckSum calculation
PandaHouse 0:6d54a81f6e58 67
PandaHouse 0:6d54a81f6e58 68 TxData[0] = 0xFA; // Header
PandaHouse 0:6d54a81f6e58 69 TxData[1] = 0xAF; // Header
PandaHouse 0:6d54a81f6e58 70 TxData[2] = ID; // ID
PandaHouse 0:6d54a81f6e58 71 TxData[3] = 0x00; // Flags
PandaHouse 0:6d54a81f6e58 72 TxData[4] = 0x1E; // Address
PandaHouse 0:6d54a81f6e58 73 TxData[5] = 0x02; // Length
PandaHouse 0:6d54a81f6e58 74 TxData[6] = 0x01; // Count
PandaHouse 0:6d54a81f6e58 75 // Data
PandaHouse 0:6d54a81f6e58 76 TxData[7] = (unsigned char)0x00FF & data; // Low byte
PandaHouse 0:6d54a81f6e58 77 TxData[8] = (unsigned char)0x00FF & (data >> 8); // Hi byte
PandaHouse 0:6d54a81f6e58 78
PandaHouse 0:6d54a81f6e58 79 // CheckSum calculation
PandaHouse 0:6d54a81f6e58 80 for(int i=2; i<=8; i++){
PandaHouse 0:6d54a81f6e58 81 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
PandaHouse 0:6d54a81f6e58 82 }
PandaHouse 0:6d54a81f6e58 83 TxData[9] = CheckSum; // Sum
PandaHouse 0:6d54a81f6e58 84 // Send Packet
PandaHouse 0:6d54a81f6e58 85 REDE = 1; // RS485 Transmitt Enable
PandaHouse 0:6d54a81f6e58 86 for(int i=0; i<=9; i++){
PandaHouse 0:6d54a81f6e58 87 device.putc(TxData[i]);
PandaHouse 0:6d54a81f6e58 88 }
PandaHouse 0:6d54a81f6e58 89 wait_us(250); // Wait for transmission
PandaHouse 0:6d54a81f6e58 90 REDE = 0; // RS485 Transmit disable
PandaHouse 0:6d54a81f6e58 91 }
PandaHouse 0:6d54a81f6e58 92 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 93 /* Funcyion : main program */
PandaHouse 0:6d54a81f6e58 94 /* NAME : main */
PandaHouse 0:6d54a81f6e58 95 /* Argument : --- */
PandaHouse 0:6d54a81f6e58 96 /* Return value : --- */
PandaHouse 0:6d54a81f6e58 97 /*-------------------------------------------------*/
PandaHouse 0:6d54a81f6e58 98 int main() {
PandaHouse 0:6d54a81f6e58 99 init(); // initialize
PandaHouse 0:6d54a81f6e58 100 torque(0x01, 0x01); // ID = 1(0x01) , torque = OFF (0x00)
PandaHouse 0:6d54a81f6e58 101 // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
PandaHouse 0:6d54a81f6e58 102 wait(1); // wait (1sec)
PandaHouse 0:6d54a81f6e58 103 while(1){
PandaHouse 0:6d54a81f6e58 104 GoalPosition(0x01,300); // ID = 1(0x01) , GoalPosition = 30.0deg(300)
PandaHouse 0:6d54a81f6e58 105 wait(1); // wait (1sec)
PandaHouse 0:6d54a81f6e58 106 GoalPosition(0x01,-300);// ID = 1(0x01) , GoalPosition = -30.0deg(-300)
PandaHouse 0:6d54a81f6e58 107 wait(1); // wait (1sec)
PandaHouse 0:6d54a81f6e58 108 }
PandaHouse 0:6d54a81f6e58 109 }