Diff: main.cpp

Revision:

0:eb69b57ab0dd

Child:

2:8b71db86f221

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 31 09:54:53 2015 +0000
@@ -0,0 +1,177 @@
+#include "mbed.h"
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+//Serial pc(USBTX, USBRX);        // tx, rx
+
+
+/*--Ethernet--------------------------------------------------------------------------------------------*/
+#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]; 
+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.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--------------------------------------------------------------------------------------------*/
+
+
+/*--Dynamixel-------------------------------------------------------------------------------------------*/
+#include "AX12.h"
+#include "MX28.h"
+
+AX12 linear (p13, p14, 0);      //直動Dynamixel
+MX28 pitch (p13, p14, 1);        //ピッチDynamixel
+MX28 yaw (p13, p14, 2);         //ヨーDynamixel
+
+int linear_goal;
+int pitch_goal;
+int 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;
+
+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);
+    wait(1);
+}
+
+void Dyna_home_position() {
+    linear.SetGoal(linear_Init);
+    pitch.SetGoal(pitch_Init);
+    yaw.SetGoal(yaw_Init);
+    wait(1); 
+}
+
+void Dyna_reset() {
+    Dyna_home_position();
+//    linear.TorqueEnable(0);
+//    pitch.TorqueEnable(0);
+//    yaw.TorqueEnable(0);
+}
+/*--Dynamixel-------------------------------------------------------------------------------------------*/
+
+
+/*--Thermal_Sensor--------------------------------------------------------------------------------------*/
+/*MEMS非接触温度センサ：形D6T-44L-06 4×4素子タイプ*/
+/*データシート：http://www.omron.co.jp/ecb/products/sensor/special/mems/pdf/AN-D6T-01JP_r2.pdf*/
+#include "MEMS.h"
+
+MEMS MEMS1(p9, p10); // sda, scl
+MEMS MEMS2(p28, p27); // sda, scl
+
+double data1[16], data2[16];
+
+void Thermal_Sensor() {
+    MEMS1.temp(data1);
+    MEMS2.temp(data2);
+}
+/*--Thermal_Sensor--------------------------------------------------------------------------------------*/
+
+
+/*--CO2_Sensor--------------------------------------------------------------------------------------*/
+/*CO2センサモジュール：A051020-AQ6B-01*/
+/*データシート：http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/
+/*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm]*/
+#include "mbed.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+AnalogIn ain(p20);
+
+float CO2_Sensor()
+{
+    float v; //生データ：電圧
+    float sensor_v,CO2;
+    
+    v=ain.read()*3.3;
+    sensor_v = v * 5.0/3.3; //電圧レベルを合わせる
+    CO2 = sensor_v * 1000 + 400;  //データシートより
+
+    return(CO2);
+}
+/*--CO2_Sensor--------------------------------------------------------------------------------------*/
+
+
+int main() {
+    
+    float lP, lV, pP, pV, pC, yP, yV, yC;
+    float CO2_data;
+    
+    Ethernet_init();    
+    Dyna_init();
+    
+    while(1) {
+        
+    /*--Dynamixel-------------------------------------------------------------------------------------------*/       
+        //現在の角度・電圧・電流を取得
+        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;
+        
+        //現在の状態を送信
+          
+        //JoyStickの値(目標角度・リセット)を取得
+               
+        //目標角速度を算出＆送信
+        myled2=1;
+        linear.SetGoal(linear_goal);
+        pitch.SetGoal(pitch_goal);
+        yaw.SetGoal(yaw_goal);
+        myled2=0;
+    /*--Dynamixel-------------------------------------------------------------------------------------------*/       
+
+    /*--Thermal_Sensor--------------------------------------------------------------------------------------*/
+        //値を取得
+        Thermal_Sensor();
+        //値を送信
+        
+    /*--Thermal_Sensor--------------------------------------------------------------------------------------*/
+
+    /*--CO2_Sensor--------------------------------------------------------------------------------------*/
+        //値を取得
+        CO2_data=CO2_Sensor();
+        //値を送信
+        
+    /*--CO2_Sensor--------------------------------------------------------------------------------------*/
+                
+        //ホームポジション
+        if() Dyna_home_position();
+        //リセット
+        if() Dyna_reset();
+        
+        //終了
+        if() {
+            Dyna_end();
+            return 0;
+        }
+    }
+}
\ No newline at end of file