雄介 太田
YOZAKURAのARMプログラム(ver.1)
Dependencies: Dynamixel EthernetInterface MEMS_Thermal_Sensor mbed-rtos SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 8:dd338c66211d
- Parent:
- 7:aea2866fa06c
- Child:
- 9:65c76b58e5b7
--- a/main.cpp Sat Apr 04 10:11:02 2015 +0000
+++ b/main.cpp Wed Apr 08 08:06:38 2015 +0000
@@ -1,3 +1,8 @@
+/*YOZAKURAのアームコード
+ UDP通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes
+ UDP通信で受け取るデータ R_Data=｛mode[2bit],linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]} 計1byte
+*/
+
#include "mbed.h"
DigitalOut myled1(LED1);
@@ -5,34 +10,30 @@
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
-
-/*--Ethernet:begin----------------------------------------------------------------------------------------*/
+/*--Ethernet:begin------------------------------------------------------------------------------------------*/
#include "mbed.h"
#include "EthernetInterface.h"
const char* ECHO_SERVER_ADDRESS = "192.168.1.90";
const int ECHO_SERVER_PORT = 60000;
-char u_buff[2];
+const char* CLIENT_ADDRESS = "192.168.1.100";
+const int CLIENT_PORT = 70000;
+
EthernetInterface eth;
UDPSocket sock;
Endpoint echo_server;
void Ethernet_init() {
- u_buff[0]=0x41; u_buff[1]=0x42;
eth.init("192.168.1.100", "255.255.255.0", ""); // Use hard IP
-
eth.connect();
- sock.init();
+ sock.bind(CLIENT_PORT);
sock.set_blocking(false);
echo_server.set_address(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
-// while(1) {
-// sock.sendTo(echo_server, u_buff, sizeof(u_buff)); wait_ms(1);
-// }
}
/*--Ethernet:end--------------------------------------------------------------------------------------------*/
@@ -42,28 +43,32 @@
#include "MX28.h"
AX12 linear (p13, p14, 0); //直動Dynamixel
+//MX28 pitch (p9, p10, 1); //ピッチDynamixel
MX28 pitch (p13, p14, 1); //ピッチDynamixel
+//MX28 yaw (p28, p27, 2); //ヨーDynamixel
MX28 yaw (p13, p14, 2); //ヨーDynamixel
-int linear_goal;
-int pitch_goal;
-int yaw_goal;
+int linear_goal, pitch_goal, yaw_goal;
//最小値、最大値、初期値を指定[unit:degree]
//MX:MultiTurnモードでは0°～5040°
-int linear_min = 100; int linear_MAX = 700; int linear_Init = 700;
-int pitch_min = 100; int pitch_MAX = 700; int pitch_Init = 700;
-int yaw_min = 180; int yaw_MAX = 180; int yaw_Init = 252;
+int linear_min = 100; int linear_MAX = 720; float linear_Speed=0.1; int linear_Init = linear_MAX;
+int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX;
+int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0;
void Dyna_init() {
- linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); linear.SetGoal(linear_Init);
- pitch.SetCWLimit(linear_min); pitch.SetCCWLimit(linear_MAX); pitch.SetGoal(pitch_Init);
- yaw.SetCWLimit(linear_min); yaw.SetCCWLimit(linear_MAX); yaw.SetGoal(yaw_Init);
+ linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX);
+ linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; linear.SetGoal(linear_Init);
+ pitch.SetCWLimit(linear_min); pitch.SetCCWLimit(linear_MAX);
+ pitch.SetCRSpeed(pitch_Speed); pitch_goal=linear_Init; pitch.SetGoal(pitch_Init);
+ yaw.SetCWLimit(linear_min); yaw.SetCCWLimit(linear_MAX);
+ yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; yaw.SetGoal(yaw_Init);
wait(1);
}
void Dyna_home_position() {
linear.SetGoal(linear_Init);
+ wait(1);
pitch.SetGoal(pitch_Init);
yaw.SetGoal(yaw_Init);
wait(1);
@@ -71,9 +76,19 @@
void Dyna_reset() {
Dyna_home_position();
-// linear.TorqueEnable(0);
-// pitch.TorqueEnable(0);
-// yaw.TorqueEnable(0);
+ linear.SetTorqueLimit(0);
+ pitch.SetTorqueLimit(0);
+ yaw.SetTorqueLimit(0);
+ linear.SetTorqueLimit(1);
+ pitch.SetTorqueLimit(1);
+ yaw.SetTorqueLimit(1);
+}
+
+void Dyna_end() {
+ Dyna_home_position();
+ linear.TorqueEnable(0);
+ pitch.TorqueEnable(0);
+ yaw.TorqueEnable(0);
}
/*--Dynamixel:end-----------------------------------------------------------------------------------------*/
@@ -86,12 +101,7 @@
MEMS MEMS1(p9, p10); // sda, scl
MEMS MEMS2(p28, p27); // sda, scl
-float data1[16], data2[16];
-
-void Thermal_Sensor() {
- MEMS1.temp(data1);
- MEMS2.temp(data2);
-}
+float ThD1[16], ThD2[16];
/*--Thermal_Sensor:end------------------------------------------------------------------------------------*/
@@ -109,7 +119,7 @@
float v; //生データ：電圧
float sensor_v,CO2;
- v=ain.read()*3.3;
+ v = ain.read()*3.3;
sensor_v = v * 5.0/3.3; //電圧レベルを合わせる
CO2 = sensor_v * 1000 + 400; //データシートより
@@ -123,57 +133,92 @@
----------------*/
int main() {
- float lP, lV, pP, pV, pC, yP, yV, yC;
+ float lP, lV, pP, pC, yP, yC;
+ char Thermo_data[100];
float CO2_data;
Ethernet_init();
Dyna_init();
+ char S_data[200], R_data[2];
+ int Joy_mode, linear_mode, pitch_mode, yaw_mode;
+
while(1) {
- /*--Dynamixel:begin---------------------------------------------------------------*/
- //現在の角度・電圧・電流を取得
- myled1=1;
- lP=linear.GetPosition(); lV=linear.GetVolts();
- pP=pitch.GetPosition(); pV=pitch.GetVolts(); pC=pitch.GetCurrent();
- yP=yaw.GetPosition(); yV=yaw.GetVolts(); yC=yaw.GetCurrent();
- myled1=0;
+ sock.receiveFrom(echo_server, R_data, sizeof(R_data));
+ wait_ms(1);
+ Joy_mode = atoi(R_data);
- //現在の状態を送信
-
- //JoyStickの値(目標角度・リセット)を取得
-
- //目標角速度を算出＆送信
- myled2=1;
- linear.SetGoal(linear_goal);
- pitch.SetGoal(pitch_goal);
- yaw.SetGoal(yaw_goal);
- myled2=0;
- /*--Dynamixel:end------------------------------------------------------------------*/
+ switch(Joy_mode){
+ case 0: //通常モード
+ /*--Dynamixel:begin---------------------------------------------------------------*/
+ //現在の角度・電圧・電流を取得
+ myled1=1;
+ lP = linear.GetPosition(); lV = linear.GetVolts();
+ pP = pitch.GetPosition(); pC = pitch.GetCurrent();
+ yP = yaw.GetPosition(); yC = yaw.GetCurrent();
+ myled1=0;
+
+ //JoyStickのモード
+ linear_mode = Joy_mode && 0x0c; linear_mode = linear_mode >> 2;
+ pitch_mode = Joy_mode && 0x30; pitch_mode = linear_mode >> 4;
+ yaw_mode = Joy_mode && 0xc0; yaw_mode = linear_mode >> 6;
- /*--Thermal_Sensor:begin-----------------------------------------------------------*/
- //値を取得
- Thermal_Sensor();
- //値を送信
-
- /*--Thermal_Sensor:end-------------------------------------------------------------*/
-
- /*--CO2_Sensor:begin---------------------------------------------------------------*/
- //値を取得
- CO2_data=CO2_Sensor();
- //値を送信
+ //目標角度へ動かす
+ myled2=1;
+ switch(linear_mode){
+ case 0: linear_goal=lP; break;
+ case 1: linear_goal++; break;
+ case 2: linear_goal--; break;
+ }
+ linear.SetGoal(linear_goal);
+ switch(pitch_mode){
+ case 0: pitch_goal=lP; break;
+ case 1: pitch_goal++; break;
+ case 2: pitch_goal--; break;
+ }
+ pitch.SetGoal(pitch_goal);
+ switch(yaw_mode){
+ case 0: yaw_goal=lP; break;
+ case 1: yaw_goal++; break;
+ case 2: yaw_goal--; break;
+ }
+ yaw.SetGoal(yaw_goal);
+ myled2=0;
+ /*--Dynamixel:end------------------------------------------------------------------*/
- /*--CO2_Sensor:end-----------------------------------------------------------------*/
+ /*--Thermal_Sensor:begin-----------------------------------------------------------*/
+ //値を取得
+ MEMS1.temp(ThD1);
+ MEMS2.temp(ThD2);
+ /*--Thermal_Sensor:end-------------------------------------------------------------*/
+
+ /*--CO2_Sensor:begin---------------------------------------------------------------*/
+ //値を取得
+ CO2_data=CO2_Sensor();
+ /*--CO2_Sensor:end-----------------------------------------------------------------*/
+
+ //値を送信
+ sprintf(Thermo_data,
+ "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
+ ThD1[0],ThD1[1],ThD1[2],ThD1[3],ThD1[4],ThD1[5],ThD1[6],ThD1[7],
+ ThD1[8],ThD1[9],ThD1[10],ThD1[11],ThD1[12],ThD1[13],ThD1[14],ThD1[15],
+ ThD2[0],ThD2[1],ThD2[2],ThD2[3],ThD2[4],ThD2[5],ThD2[6],ThD2[7],
+ ThD2[8],ThD2[9],ThD2[10],ThD2[11],ThD2[12],ThD2[13],ThD2[14],ThD2[15]);
+ sprintf(S_data, "%f %f %f %f %f %f %s %f",lP,lV,pP,pC,yP,yC,Thermo_data,CO2_data);
+ sock.sendTo(echo_server, S_data, sizeof(S_data));
+
+ break;
+
+
+ case 1: //ホームポジション
+ Dyna_home_position(); break;
- //ホームポジション
- if() Dyna_home_position();
- //リセット
- if() Dyna_reset();
-
- //終了
- if() {
- Dyna_end();
- return 0;
+ case 2: //リセット
+ Dyna_reset(); break;
+
+ case 3: //終了
+ Dyna_end(); return 0;
}
}
}
\ No newline at end of file