main.cpp

00001 #include "mbed.h"
00002 #include "Futaba485.h"
00003 
00004 
00005 DigitalOut led1(LED1);
00006 DigitalOut led2(LED2);
00007 DigitalOut led3(LED3);
00008 DigitalOut led4(LED4);
00009 Serial PC(USBTX,USBRX);
00010 Futaba futaba(p13,p14,p15);
00011 
00012 
00013 int main() {
00014     futaba.init();                 // initialize
00015     futaba.torqueOn(0x01);         // ID = 1(0x01) 
00016     futaba.torqueOn(0x02);
00017     futaba.setTmax(0x01,0x64);     //set torque max 00H-64H 
00018     futaba.setTslope(0x01, 0xf, 0xf); //set torque slope  ID,CWslope,CCWslope  00H~FFH 0deg~255deg 0x1E:30deg
00019     int futaba_angle=0;
00020     
00021     while(1) {
00022         //----- ID1
00023         futaba.torqueOn(0x01);// ID = 1(0x01)
00024         wait(0.1);
00025         futaba_angle=3000;
00026         led1 = 1;
00027         futaba.setPosition(0x01,futaba_angle); // ID = 1(0x01) , GoalPosition :10 = 1deg
00028         wait(1);
00029         futaba_angle=-3000;
00030         futaba.setPosition(0x01,futaba_angle); // ID = 1(0x01) , GoalPosition :10 = 1deg
00031         led1 = 0;
00032         wait(1);
00033         futaba.torqueOff(0x01);// ID = 1(0x01)
00034         
00035         //----- ID2 
00036         futaba.torqueOn(0x02);// ID = 2(0x02)
00037         wait(0.1);
00038         futaba_angle=3000;
00039         led2 = 1;
00040         futaba.setPosition(0x02,futaba_angle); // ID = 2(0x02) , GoalPosition :10 = 1deg
00041         wait(1);
00042         futaba_angle=-3000;
00043         futaba.setPosition(0x02,futaba_angle); // ID = 2(0x02) , GoalPosition :10 = 1deg
00044         led2 = 0;
00045         wait(1);
00046         futaba.torqueOff(0x02);
00047     }
00048 }