からっ風Pussyfootの回路とプログラム
今年はついに本大会で自作回路を実装しました！
去年からやり始めて作り直し数回、ESCごと燃やすこと1回
機能は少ないですが、やっと安心して使えるものになりました。
これからいろいろと機能を追加していく予定です。
１．回路機能○電源系
入力は13.2Vで、サーボとDCDCに供給
（サーボの定格12Vをオーバーしてるので、壊れても自己責任っていうことで）
受信機とmbedへはDCDC（絶縁型）で落とした５Vを利用
絶縁型なのでもしもの時も安心
○入力信号：受信機からのPWM（1ch)
○出力信号：RS485
サーボへのRS485はICを利用
詳細は双葉コマンドサーボのHP（http://futaba.co.jp/robot/systems）の下の方を参照で
２．プログラム
長いので詳細は続きから
からっ風本体のプログラム
色々間に合わなかったので受信機からのPWM信号を角度に変換してサーボに送っているだけです。
アームの角度リミッタ機能などなど秋に作って今回積んでいない機能などありますので、無駄なコードもあります。
QEIはアームのエンコーダのカウンタ用ですが、今年はエンコーダが間に合わなかったので使っていません。
main.cpp
-----------------------------------------------------
#include "mbed.h"
#include "Futaba485.h"
#include "QEI.h"
/*----
------*/
//----------define------------
//-------PWM output
#define neutral 1520 //PWM output neutral (us)
#define count_to_angle 360/1040 //convert count to angle
// encoder :100ppr
// main arm gear:26/10
// arm:1rot -> encoder:2.6rot // *100ppr *4 = 1040 ppr/arm
// 360/1040[deg/pulth] = 0.346 deg/pulth
#define PID_P 1
//-------------pin SetUP------------
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Serial PC(USBTX,USBRX);
Futaba futaba(p13,p14,p15);
QEI enc_arm(p8, p9, NC, 100, QEI::X4_ENCODING);
//PwmOut pwm_arm(p21);
//PwmOut pwm2(p22);
//PwmOut pwm3(p23);
InterruptIn ch1(p25);
//InterruptIn ch2(p26);
//InterruptIn ch3(p27);
//InterruptIn ch4(p28);
//InterruptIn ch5(p29);
//InterruptIn ch6(p30);
//----------Global Variable------------------
Timer t1,t3,t5,roop;//,test;
int ch1_time,ch2_time,ch3_time,ch4_time,ch5_time,ch6_time;
//------------InterruptIn-------------
void timer1_start() { //main arm
t1.start();
}
void timer1_stop() {
t1.stop();
ch1_time=int(t1.read()*1000*1000);//pulse time us
t1.reset();
}
/*
void timer3_start() { //arm unit
t3.start();
}
void timer3_stop() {
t3.stop();
ch3_time=int(t3.read()*1000*1000);//pulse time us
t3.reset();
}
void timer5_start() { //arm unit
t5.start();
}
void timer5_stop() {
t5.stop();
ch5_time=int(t5.read()*1000*1000);//pulse time us
t5.reset();
}
*/
//--------------------main-------------
int main() {
int PWM_width1,PWM_width3;
int futaba_angle;
int arm_pos;//,get_arm_pos=0;
int pwidth; //ESC PWM width
//int test=0;
//int start_wait=0,start=0,start_old=1,start_bit=0;
int pass_time=0;
PC.printf("-----------------------\n");
PC.printf("mbed for KARAKKAZE Pussyfoot\n");
PC.printf("-----------------------\n");
wait(1);
//Futaba servo setup
futaba.init(); // initialize
futaba.torqueOn(0x01); // ID = 1(0x01) , torque = OFF (0x00)
futaba.setTmax(0x01,0x64); //set torque max 00H-64H
futaba.setTslope(0x01, 0xf, 0xf); //set torque slope ID,CWslope,CCWslope 00H~FFH 0deg~255deg 0x1E:30deg
// input pwm setup
ch1.rise(&timer1_start);
ch1.fall(&timer1_stop);
// ch3.rise(&timer3_start);
// ch3.fall(&timer3_stop);
// ch5.rise(&timer5_start);
// ch5.fall(&timer5_stop);
while(1){
///////======================== arm unit ===================================
//---Futaba_servo-----------
PWM_width1 = ch1_time-neutral;
futaba_angle=int(3750.0*PWM_width1*1/1000); //3750=1500[0.1deg]/0.4[ms] //0.4ms:pwm peak-to-peak
if(futaba_angle>0){
futaba_angle=int(0.5*futaba_angle);
}else{
futaba_angle=int(0.3*futaba_angle);
}
if (futaba_angle>900){//data limit
futaba_angle = 900;
}else if(futaba_angle<-300){
futaba_angle = -300;
}
//PC.printf("Futaba_servo angle:%d *0.1deg \n", futaba_angle);
futaba.setPosition(0x01,-futaba_angle); // ID = 1(0x01) , GoalPosition :10 = 1deg
wait_ms(5);
// -------roop time mesure------------------
roop.stop();
pass_time = roop.read_us();
roop.reset();
PC.printf("RoopTime:%dus ", pass_time);
PC.printf("\n");
}
}
--------------------------------------------
次はコマンドサーボ関係
当時いい感じのライブラリが見つからなかったので、双葉のコマンドサーボ用のmbedライブラリを作りました。
双葉HPのプログラムを参考にしながら書いています。
ご自由にご利用ください（自己責任で）
バグや変なコード等ありましたらご指摘いただけるとありがたいです。
今のところ実装している機能は下記のものFutaba(PinName tx ,PinName rx, PinName REDE); //ピン設定 TxRxはシリアルピンから、REDEはIOから選択
init(); //初期化
setTmax(unsigned char ID, unsigned char data); //任意のIDのサーボの最大トルクを設定
torqueOn (unsigned char ID); //任意のIDサーボのトルクをON
torqueOff (unsigned char ID); //任意のIDのサーボのトルクをON
setPosition (unsigned char ID, int data); //任意のサーボの位置指令を送信
setPosition_long123 (int data1,int data2, int data3); //ID 1~3 //ID1～3のサーボの位置指令を同時に送信
setTslope(unsigned char ID, unsigned char CW, unsigned char CCW);/任意のサーボのトルクスロープを設定。CW,CCWでそれぞれ入力
色々と機能実装したいときは説明書や解説ページとにらめっこしてください。
Futaba485.h
-----------------------------------------
#ifndef MBED_FUTABA_SERVO_H
#define MBED_FUTABA_SERVO_H
#include "mbed.h"
class Futaba {
private:
Serial _device; // tx, rx
DigitalOut _REDE; //transmitt enable or disenable
public:
Futaba(PinName tx ,PinName rx, PinName REDE);
void init();
void setTmax(unsigned char ID, unsigned char data);
void torqueOn (unsigned char ID);
void torqueOff (unsigned char ID);
void setPosition (unsigned char ID, int data);
void setPosition_long123 (int data1,int data2, int data3); //ID 1~3
void setTslope(unsigned char ID, unsigned char CW, unsigned char CCW);//00H~FFH 0deg~255deg
};
#endif
------------------------------------------------------------
Futaba485.cpp
---------------------------------------------------------------
#include "Futaba485.h"
#include "mbed.h"
Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
}
void Futaba::init(void){
_device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
_REDE = 0; // RS485 Transmit disable
}
void Futaba::setTmax (unsigned char ID, unsigned char data){
unsigned char TxData[9]; // TransmitByteData [9byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x23; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = data; // Data
// CheckSum calculation
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[8] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmit Enable
for(int i=0; i<=8; i++){
_device.putc(TxData[i]);
}
wait_us(250); // Wait for transmission
_REDE = 0; // RS485 Transmitt disable
}
void Futaba::torqueOn (unsigned char ID){
unsigned char TxData[9]; // TransmitByteData [9byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x24; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = 0x01; // Data
// CheckSum calculation
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[8] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmit Enable
for(int i=0; i<=8; i++){
_device.putc(TxData[i]);
}
wait_us(250); // Wait for transmission
_REDE = 0; // RS485 Transmitt disable
}
void Futaba::torqueOff (unsigned char ID){
unsigned char TxData[9]; // TransmitByteData [9byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x24; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = 0x00; // Data
// CheckSum calculation
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[8] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmit Enable
for(int i=0; i<=8; i++){
_device.putc(TxData[i]);
}
wait_us(250); // Wait for transmission
_REDE = 0; // RS485 Transmitt disable
}
void Futaba::setPosition (unsigned char ID, int data){
unsigned char TxData[10]; // TransmitByteData [10byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x1E; // Address
TxData[5] = 0x02; // Length
TxData[6] = 0x01; // Count
// Data
TxData[7] = (unsigned char)0x00FF & data; // Low byte
TxData[8] = (unsigned char)0x00FF & (data >> 8); // Hi byte
// CheckSum calculation
for(int i=2; i<=8; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[9] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmitt Enable
for(int i=0; i<=9; i++){
_device.putc(TxData[i]);
}
wait_us(250); // Wait for transmission
_REDE = 0; // RS485 Transmit disable
}
void Futaba::setPosition_long123 (int data1,int data2, int data3){
unsigned char TxData[17]; // TransmitByteData [10byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = 0x00; // ID long packet
TxData[3] = 0x00; // Flags
TxData[4] = 0x1E; // Address
TxData[5] = 0x03; // Length
TxData[6] = 0x03; // Count
TxData[7] = 0x01; //Servo ID1
TxData[8] = (unsigned char)0x00FF & data1; // Low byte
TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
TxData[10] = 0x02; //Servo ID2
TxData[11] = (unsigned char)0x00FF & data2; // Low byte
TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
TxData[13] = 0x03; //Servo ID3
TxData[14] = (unsigned char)0x00FF & data3; // Low byte
TxData[15] = (unsigned char)0x00FF & (data3 >> 8); // Hi byte
// CheckSum calculation
for(int i=2; i<=15; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[16] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmitt Enable
for(int i=0; i<=16; i++){
_device.putc(TxData[i]);
}
wait_us(500); // Wait for transmission
_REDE = 0; // RS485 Transmit disable
}
void Futaba::setTslope (unsigned char ID, unsigned char CW, unsigned char CCW){
unsigned char TxData[10]; // TransmitByteData [9byte]
unsigned char CheckSum = 0; // CheckSum calculation
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x1A; // Address
TxData[5] = 0x02; // Length
TxData[6] = 0x01; // Count
TxData[7] = CW; // Data
TxData[8] = CCW; // Data
// CheckSum calculation
for(int i=2; i<=8; i++){
CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
}
TxData[9] = CheckSum; // Sum
// Send Packet
_REDE = 1; // RS485 Transmit Enable
for(int i=0; i<=9; i++){
_device.putc(TxData[i]);
}
wait_us(250); // Wait for transmission
_REDE = 0; // RS485 Transmitt disable
}
---------------------------------------------------------------------------
以上！ 簡単なまとめでした。
自作回路があると自由度高くなって妄想がはかどりますｗ
