雄介 太田
Control YOZAKURA arm vir USB
Dependencies: Dynamixel MEMS_Thermal_Sensor SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 4:dd8c3543adc7
- Parent:
- 3:6dbba9951f9e
- Child:
- 5:a6d9a1dec391
--- 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;