雄介 太田
YOZAKURAのARMプログラム(ver.1)
Dependencies: Dynamixel EthernetInterface MEMS_Thermal_Sensor mbed-rtos SerialHalfDuplex mbed
Revision 16:b01cfa36fa26, committed 2015-04-17
- Comitter:
- yusuke_kyo
- Date:
- Fri Apr 17 02:37:00 2015 +0000
- Parent:
- 15:d62819013445
- Commit message:
- function
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d62819013445 -r b01cfa36fa26 main.cpp
--- a/main.cpp Thu Apr 16 02:37:43 2015 +0000
+++ b/main.cpp Fri Apr 17 02:37:00 2015 +0000
@@ -24,7 +24,6 @@
Endpoint echo_server;
void Ethernet_init() {
-
eth.init("192.168.1.100", "255.255.255.0", ""); // Use hard IP
eth.connect();
@@ -32,7 +31,6 @@
sock.set_blocking(false);
echo_server.set_address(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
-
}
/*--Ethernet:end--------------------------------------------------------------------------------------------*/
@@ -66,12 +64,49 @@
wait(1);
}
+void GetDyna(char* data) {
+ float lP, lV, pP, pC, yP, yC;
+
+ lP = linear.GetPosition(); lV = linear.GetVolts();
+ pP = pitch.GetPosition(); pC = pitch.GetCurrent();
+ yP = yaw.GetPosition(); yC = yaw.GetCurrent();
+
+ linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定
+
+ sprintf(data,"%f %f %f %f %f %f ",lP,lV,pP,pC,yP,yC);
+}
+
+void SetGoalDyna(int l_mode, int p_mode, int y_mode) {
+ switch(l_mode){
+ case 0: break;
+ case 1: linear.SetTorqueLimit(1); linear_goal++; break;
+ case 2: linear.SetTorqueLimit(1); linear_goal--; break;
+ }
+ linear.SetGoal(linear_goal);
+
+ switch(p_mode){
+ case 0: break;
+ case 1: pitch_goal++; break;
+ case 2: pitch_goal--; break;
+ }
+ pitch.SetGoal(pitch_goal);
+
+ switch(y_mode){
+ case 0: break;
+ case 1: yaw_goal++; break;
+ case 2: yaw_goal--; break;
+ }
+ yaw.SetGoal(yaw_goal);
+}
+
void Dyna_home_position() {
+ float lp;
linear.SetGoal(linear_Init);
- wait(3);
- pitch.SetGoal(pitch_Init);
- yaw.SetGoal(yaw_Init);
- wait(2);
+ lp=linear.GetPosition();
+ if(lp > linear_MAX - 30) { //ある程度縮んだら
+ pitch.SetGoal(pitch_Init);
+ yaw.SetGoal(yaw_Init);
+ }
}
void Dyna_reset() {
@@ -81,7 +116,6 @@
}
void Dyna_end() {
- Dyna_home_position();
RelaySwitch = 0;
}
/*--Dynamixel:end-----------------------------------------------------------------------------------------*/
@@ -94,6 +128,23 @@
MEMS MEMS1(p9, p10); // sda, scl
MEMS MEMS2(p28, p27); // sda, scl
+
+void GetThermo(char* data) {
+ float ThD1[16], ThD2[16];
+ char con[10];
+
+ MEMS1.temp(ThD1);
+ MEMS2.temp(ThD2);
+
+ for(int i=0;i<16;i++){
+ sprintf(con,"%3.1f ",ThD1[i]);
+ strcat(data,con);
+ }
+ for(int i=0;i<16;i++){
+ sprintf(con,"%3.1f ",ThD2[i]);
+ strcat(data,con);
+ }
+}
/*--Thermal_Sensor:end------------------------------------------------------------------------------------*/
@@ -105,8 +156,7 @@
AnalogIn ain(p20);
-float CO2_Sensor()
-{
+float GetCO2() {
float v; //生データ：電圧
float sensor_v,CO2;
@@ -123,9 +173,7 @@
MAIN ROOP
----------------*/
int main() {
-
- float lP, lV, pP, pC, yP, yC;
- float ThD1[16], ThD2[16];
+ char Dyna_data[100];
char Thermo_data[100];
float CO2_data;
@@ -148,69 +196,39 @@
case 0: //通常モード
stdio_mutex.lock();
/*--Dynamixel:begin---------------------------------------------------------------*/
- //現在の角度・電圧・電流を取得
- myled1=1;
- lP = linear.GetPosition(); lV = linear.GetVolts();
- pP = pitch.GetPosition(); pC = pitch.GetCurrent();
- yP = yaw.GetPosition(); yC = yaw.GetCurrent();
- myled1=0;
-
//Dynamixelへの命令を判定
linear_mode = R_data[0] & 0x0c; linear_mode = linear_mode >> 2;
pitch_mode = R_data[0] & 0x30; pitch_mode = linear_mode >> 4;
yaw_mode = R_data[0] & 0xc0; yaw_mode = linear_mode >> 6;
+ //現在の角度・電圧・電流を取得
+ myled1=1;
+ GetDyna(Dyna_data);
+ myled1=0;
+
//目標角度を変更
myled2=1;
- linear_goal=lP;
- switch(linear_mode){
- case 0: break;
- case 1: linear.SetTorqueLimit(1); linear_goal++; break;
- case 2: linear.SetTorqueLimit(1); linear_goal--; break;
- }
- linear.SetGoal(linear_goal);
- pitch_goal=pP;
- switch(pitch_mode){
- case 0: break;
- case 1: pitch_goal++; break;
- case 2: pitch_goal--; break;
- }
- pitch.SetGoal(pitch_goal);
- yaw_goal=yP;
- switch(yaw_mode){
- case 0: break;
- case 1: yaw_goal++; break;
- case 2: yaw_goal--; break;
- }
- yaw.SetGoal(yaw_goal);
+ SetGoalDyna(linear_mode, pitch_mode, yaw_mode);
myled2=0;
/*--Dynamixel:end------------------------------------------------------------------*/
/*--Thermal_Sensor:begin-----------------------------------------------------------*/
//値を取得
- MEMS1.temp(ThD1);
- MEMS2.temp(ThD2);
+ GetThermo(Thermo_data);
/*--Thermal_Sensor:end-------------------------------------------------------------*/
/*--CO2_Sensor:begin---------------------------------------------------------------*/
//値を取得
- CO2_data=CO2_Sensor();
+ CO2_data=GetCO2();
/*--CO2_Sensor:end-----------------------------------------------------------------*/
//値を送信
myled3=1;
- 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);
+ sprintf(S_data, "%s%s%f",Dyna_data,Thermo_data,CO2_data);
stdio_mutex.unlock();
sock.sendTo(echo_server, S_data, sizeof(S_data));
myled3=0;
break;
-
case 1: //ホームポジション
stdio_mutex.lock(); Dyna_home_position(); stdio_mutex.unlock(); break;