Control YOZAKURA arm vir USB
Dependencies: Dynamixel MEMS_Thermal_Sensor SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 4:dd8c3543adc7
- Parent:
- 3:6dbba9951f9e
- Child:
- 5:a6d9a1dec391
diff -r 6dbba9951f9e -r dd8c3543adc7 main.cpp --- a/main.cpp Thu Apr 16 02:18:04 2015 +0000 +++ b/main.cpp Fri Apr 17 02:39:00 2015 +0000 @@ -28,7 +28,7 @@ void Dyna_init() { low = 0; RelaySwitch = 1; - linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); + 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); @@ -37,12 +37,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() { @@ -52,7 +89,6 @@ } void Dyna_end() { - Dyna_home_position(); RelaySwitch = 0; } /*--Dynamixel:end-----------------------------------------------------------------------------------------*/ @@ -65,19 +101,35 @@ 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------------------------------------------------------------------------------------*/ /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/ /*CO2センサモジュール:A051020-AQ6B-01*/ /*データシート:http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/ -/*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは5400[ppm]まで*/ +/*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは3000[ppm]まで*/ #include "mbed.h" AnalogIn ain(p20); -float CO2_Sensor() -{ +float GetCO2() { float v; //生データ:電圧 float sensor_v,CO2; @@ -94,9 +146,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; @@ -116,69 +166,39 @@ 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; - //Dynamixelへの命令を判定 linear_mode = R_data & 0x0c; linear_mode = linear_mode >> 2; pitch_mode = R_data & 0x30; pitch_mode = linear_mode >> 4; yaw_mode = R_data & 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); // sock.sendTo(echo_server, S_data, sizeof(S_data)); pc.printf("%s\n",S_data); myled3=0; break; - case 1: //ホームポジション Dyna_home_position(); break;