File content as of revision 3:ab3979b1ae40:

#include "mbed.h"

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
//Serial pc(USBTX, USBRX);        // tx, rx


/*--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]; 
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:end--------------------------------------------------------------------------------------------*/


/*--Dynamixel:begin-----------------------------------------------------------------------------------------*/
#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:end-----------------------------------------------------------------------------------------*/


/*--Thermal_Sensor:begin----------------------------------------------------------------------------------*/
/*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: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]*/
#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:end--------------------------------------------------------------------------------------*/


/*---------------
    MAIN ROOP
----------------*/
int main() {
    
    float lP, lV, pP, pV, pC, yP, yV, yC;
    float CO2_data;
    
    Ethernet_init();    
    Dyna_init();
    
    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;
        
        //現在の状態を送信
          
        //JoyStickの値(目標角度・リセット)を取得
               
        //目標角速度を算出＆送信
        myled2=1;
        linear.SetGoal(linear_goal);
        pitch.SetGoal(pitch_goal);
        yaw.SetGoal(yaw_goal);
        myled2=0;
    /*--Dynamixel:end--------------------------------------------------------------------------------------*/       

    /*--Thermal_Sensor:begin-------------------------------------------------------------------------------*/
        //値を取得
        Thermal_Sensor();
        //値を送信
        
    /*--Thermal_Sensor:end---------------------------------------------------------------------------------*/

    /*--CO2_Sensor:begin-----------------------------------------------------------------------------------*/
        //値を取得
        CO2_data=CO2_Sensor();
        //値を送信
        
    /*--CO2_Sensor:end-------------------------------------------------------------------------------------*/
                
        //ホームポジション
        if() Dyna_home_position();
        //リセット
        if() Dyna_reset();
        
        //終了
        if() {
            Dyna_end();
            return 0;
        }
    }
}