![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
YOZAKURAのARMプログラム(ver.1)
Dependencies: Dynamixel EthernetInterface MEMS_Thermal_Sensor mbed-rtos SerialHalfDuplex mbed
main.cpp@2:8b71db86f221, 2015-04-01 (annotated)
- Committer:
- yusuke_kyo
- Date:
- Wed Apr 01 03:52:21 2015 +0000
- Revision:
- 2:8b71db86f221
- Parent:
- 0:eb69b57ab0dd
- Child:
- 3:ab3979b1ae40
begin,end????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yusuke_kyo | 0:eb69b57ab0dd | 1 | #include "mbed.h" |
yusuke_kyo | 0:eb69b57ab0dd | 2 | |
yusuke_kyo | 0:eb69b57ab0dd | 3 | DigitalOut myled1(LED1); |
yusuke_kyo | 0:eb69b57ab0dd | 4 | DigitalOut myled2(LED2); |
yusuke_kyo | 0:eb69b57ab0dd | 5 | DigitalOut myled3(LED3); |
yusuke_kyo | 0:eb69b57ab0dd | 6 | DigitalOut myled4(LED4); |
yusuke_kyo | 0:eb69b57ab0dd | 7 | //Serial pc(USBTX, USBRX); // tx, rx |
yusuke_kyo | 0:eb69b57ab0dd | 8 | |
yusuke_kyo | 0:eb69b57ab0dd | 9 | |
yusuke_kyo | 2:8b71db86f221 | 10 | /*--Ethernet:begin--------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 11 | #include "mbed.h" |
yusuke_kyo | 0:eb69b57ab0dd | 12 | #include "EthernetInterface.h" |
yusuke_kyo | 0:eb69b57ab0dd | 13 | |
yusuke_kyo | 0:eb69b57ab0dd | 14 | const char* ECHO_SERVER_ADDRESS = "192.168.1.90"; |
yusuke_kyo | 0:eb69b57ab0dd | 15 | const int ECHO_SERVER_PORT = 60000; |
yusuke_kyo | 0:eb69b57ab0dd | 16 | |
yusuke_kyo | 0:eb69b57ab0dd | 17 | char u_buff[2]; |
yusuke_kyo | 0:eb69b57ab0dd | 18 | EthernetInterface eth; |
yusuke_kyo | 0:eb69b57ab0dd | 19 | UDPSocket sock; |
yusuke_kyo | 0:eb69b57ab0dd | 20 | Endpoint echo_server; |
yusuke_kyo | 0:eb69b57ab0dd | 21 | |
yusuke_kyo | 0:eb69b57ab0dd | 22 | void Ethernet_init() { |
yusuke_kyo | 0:eb69b57ab0dd | 23 | |
yusuke_kyo | 0:eb69b57ab0dd | 24 | u_buff[0]=0x41; u_buff[1]=0x42; |
yusuke_kyo | 0:eb69b57ab0dd | 25 | eth.init("192.168.1.100", "255.255.255.0", ""); // Use hard IP |
yusuke_kyo | 0:eb69b57ab0dd | 26 | |
yusuke_kyo | 0:eb69b57ab0dd | 27 | eth.connect(); |
yusuke_kyo | 0:eb69b57ab0dd | 28 | |
yusuke_kyo | 0:eb69b57ab0dd | 29 | sock.init(); |
yusuke_kyo | 0:eb69b57ab0dd | 30 | sock.set_blocking(false); |
yusuke_kyo | 0:eb69b57ab0dd | 31 | |
yusuke_kyo | 0:eb69b57ab0dd | 32 | echo_server.set_address(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT); |
yusuke_kyo | 0:eb69b57ab0dd | 33 | |
yusuke_kyo | 0:eb69b57ab0dd | 34 | // while(1) { |
yusuke_kyo | 0:eb69b57ab0dd | 35 | // sock.sendTo(echo_server, u_buff, sizeof(u_buff)); wait_ms(1); |
yusuke_kyo | 0:eb69b57ab0dd | 36 | // } |
yusuke_kyo | 0:eb69b57ab0dd | 37 | } |
yusuke_kyo | 2:8b71db86f221 | 38 | /*--Ethernet:end--------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 39 | |
yusuke_kyo | 0:eb69b57ab0dd | 40 | |
yusuke_kyo | 2:8b71db86f221 | 41 | /*--Dynamixel:begin-------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 42 | #include "AX12.h" |
yusuke_kyo | 0:eb69b57ab0dd | 43 | #include "MX28.h" |
yusuke_kyo | 0:eb69b57ab0dd | 44 | |
yusuke_kyo | 0:eb69b57ab0dd | 45 | AX12 linear (p13, p14, 0); //直動Dynamixel |
yusuke_kyo | 0:eb69b57ab0dd | 46 | MX28 pitch (p13, p14, 1); //ピッチDynamixel |
yusuke_kyo | 0:eb69b57ab0dd | 47 | MX28 yaw (p13, p14, 2); //ヨーDynamixel |
yusuke_kyo | 0:eb69b57ab0dd | 48 | |
yusuke_kyo | 0:eb69b57ab0dd | 49 | int linear_goal; |
yusuke_kyo | 0:eb69b57ab0dd | 50 | int pitch_goal; |
yusuke_kyo | 0:eb69b57ab0dd | 51 | int yaw_goal; |
yusuke_kyo | 0:eb69b57ab0dd | 52 | |
yusuke_kyo | 0:eb69b57ab0dd | 53 | //最小値、最大値、初期値を指定[unit:degree] |
yusuke_kyo | 0:eb69b57ab0dd | 54 | //MX:MultiTurnモードでは0°~5040° |
yusuke_kyo | 0:eb69b57ab0dd | 55 | int linear_min = 100; int linear_MAX = 700; int linear_Init = 700; |
yusuke_kyo | 0:eb69b57ab0dd | 56 | int pitch_min = 100; int pitch_MAX = 700; int pitch_Init = 700; |
yusuke_kyo | 0:eb69b57ab0dd | 57 | int yaw_min = 180; int yaw_MAX = 180; int yaw_Init = 252; |
yusuke_kyo | 0:eb69b57ab0dd | 58 | |
yusuke_kyo | 0:eb69b57ab0dd | 59 | void Dyna_init() { |
yusuke_kyo | 0:eb69b57ab0dd | 60 | linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); linear.SetGoal(linear_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 61 | pitch.SetCWLimit(linear_min); pitch.SetCCWLimit(linear_MAX); pitch.SetGoal(pitch_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 62 | yaw.SetCWLimit(linear_min); yaw.SetCCWLimit(linear_MAX); yaw.SetGoal(yaw_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 63 | wait(1); |
yusuke_kyo | 0:eb69b57ab0dd | 64 | } |
yusuke_kyo | 0:eb69b57ab0dd | 65 | |
yusuke_kyo | 0:eb69b57ab0dd | 66 | void Dyna_home_position() { |
yusuke_kyo | 0:eb69b57ab0dd | 67 | linear.SetGoal(linear_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 68 | pitch.SetGoal(pitch_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 69 | yaw.SetGoal(yaw_Init); |
yusuke_kyo | 0:eb69b57ab0dd | 70 | wait(1); |
yusuke_kyo | 0:eb69b57ab0dd | 71 | } |
yusuke_kyo | 0:eb69b57ab0dd | 72 | |
yusuke_kyo | 0:eb69b57ab0dd | 73 | void Dyna_reset() { |
yusuke_kyo | 0:eb69b57ab0dd | 74 | Dyna_home_position(); |
yusuke_kyo | 0:eb69b57ab0dd | 75 | // linear.TorqueEnable(0); |
yusuke_kyo | 0:eb69b57ab0dd | 76 | // pitch.TorqueEnable(0); |
yusuke_kyo | 0:eb69b57ab0dd | 77 | // yaw.TorqueEnable(0); |
yusuke_kyo | 0:eb69b57ab0dd | 78 | } |
yusuke_kyo | 2:8b71db86f221 | 79 | /*--Dynamixel:end-------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 80 | |
yusuke_kyo | 0:eb69b57ab0dd | 81 | |
yusuke_kyo | 2:8b71db86f221 | 82 | /*--Thermal_Sensor:begin--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 83 | /*MEMS非接触温度センサ:形D6T-44L-06 4×4素子タイプ*/ |
yusuke_kyo | 0:eb69b57ab0dd | 84 | /*データシート:http://www.omron.co.jp/ecb/products/sensor/special/mems/pdf/AN-D6T-01JP_r2.pdf*/ |
yusuke_kyo | 0:eb69b57ab0dd | 85 | #include "MEMS.h" |
yusuke_kyo | 0:eb69b57ab0dd | 86 | |
yusuke_kyo | 0:eb69b57ab0dd | 87 | MEMS MEMS1(p9, p10); // sda, scl |
yusuke_kyo | 0:eb69b57ab0dd | 88 | MEMS MEMS2(p28, p27); // sda, scl |
yusuke_kyo | 0:eb69b57ab0dd | 89 | |
yusuke_kyo | 0:eb69b57ab0dd | 90 | double data1[16], data2[16]; |
yusuke_kyo | 0:eb69b57ab0dd | 91 | |
yusuke_kyo | 0:eb69b57ab0dd | 92 | void Thermal_Sensor() { |
yusuke_kyo | 0:eb69b57ab0dd | 93 | MEMS1.temp(data1); |
yusuke_kyo | 0:eb69b57ab0dd | 94 | MEMS2.temp(data2); |
yusuke_kyo | 0:eb69b57ab0dd | 95 | } |
yusuke_kyo | 2:8b71db86f221 | 96 | /*--Thermal_Sensor:end--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 97 | |
yusuke_kyo | 0:eb69b57ab0dd | 98 | |
yusuke_kyo | 2:8b71db86f221 | 99 | /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 100 | /*CO2センサモジュール:A051020-AQ6B-01*/ |
yusuke_kyo | 0:eb69b57ab0dd | 101 | /*データシート:http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/ |
yusuke_kyo | 0:eb69b57ab0dd | 102 | /*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm]*/ |
yusuke_kyo | 0:eb69b57ab0dd | 103 | #include "mbed.h" |
yusuke_kyo | 0:eb69b57ab0dd | 104 | |
yusuke_kyo | 0:eb69b57ab0dd | 105 | Serial pc(USBTX, USBRX); // tx, rx |
yusuke_kyo | 0:eb69b57ab0dd | 106 | AnalogIn ain(p20); |
yusuke_kyo | 0:eb69b57ab0dd | 107 | |
yusuke_kyo | 0:eb69b57ab0dd | 108 | float CO2_Sensor() |
yusuke_kyo | 0:eb69b57ab0dd | 109 | { |
yusuke_kyo | 0:eb69b57ab0dd | 110 | float v; //生データ:電圧 |
yusuke_kyo | 0:eb69b57ab0dd | 111 | float sensor_v,CO2; |
yusuke_kyo | 0:eb69b57ab0dd | 112 | |
yusuke_kyo | 0:eb69b57ab0dd | 113 | v=ain.read()*3.3; |
yusuke_kyo | 0:eb69b57ab0dd | 114 | sensor_v = v * 5.0/3.3; //電圧レベルを合わせる |
yusuke_kyo | 0:eb69b57ab0dd | 115 | CO2 = sensor_v * 1000 + 400; //データシートより |
yusuke_kyo | 0:eb69b57ab0dd | 116 | |
yusuke_kyo | 0:eb69b57ab0dd | 117 | return(CO2); |
yusuke_kyo | 0:eb69b57ab0dd | 118 | } |
yusuke_kyo | 2:8b71db86f221 | 119 | /*--CO2_Sensor:end--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 120 | |
yusuke_kyo | 0:eb69b57ab0dd | 121 | |
yusuke_kyo | 2:8b71db86f221 | 122 | /*--------------- |
yusuke_kyo | 2:8b71db86f221 | 123 | MAIN ROOP |
yusuke_kyo | 2:8b71db86f221 | 124 | ----------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 125 | int main() { |
yusuke_kyo | 0:eb69b57ab0dd | 126 | |
yusuke_kyo | 0:eb69b57ab0dd | 127 | float lP, lV, pP, pV, pC, yP, yV, yC; |
yusuke_kyo | 0:eb69b57ab0dd | 128 | float CO2_data; |
yusuke_kyo | 0:eb69b57ab0dd | 129 | |
yusuke_kyo | 0:eb69b57ab0dd | 130 | Ethernet_init(); |
yusuke_kyo | 0:eb69b57ab0dd | 131 | Dyna_init(); |
yusuke_kyo | 0:eb69b57ab0dd | 132 | |
yusuke_kyo | 0:eb69b57ab0dd | 133 | while(1) { |
yusuke_kyo | 0:eb69b57ab0dd | 134 | |
yusuke_kyo | 2:8b71db86f221 | 135 | /*--Dynamixel:begin-------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 136 | //現在の角度・電圧・電流を取得 |
yusuke_kyo | 0:eb69b57ab0dd | 137 | myled1=1; |
yusuke_kyo | 0:eb69b57ab0dd | 138 | lP=linear.GetPosition(); lV=linear.GetVolts(); |
yusuke_kyo | 0:eb69b57ab0dd | 139 | pP=pitch.GetPosition(); pV=pitch.GetVolts(); pC=pitch.GetCurrent(); |
yusuke_kyo | 0:eb69b57ab0dd | 140 | yP=yaw.GetPosition(); yV=yaw.GetVolts(); yC=yaw.GetCurrent(); |
yusuke_kyo | 0:eb69b57ab0dd | 141 | myled1=0; |
yusuke_kyo | 0:eb69b57ab0dd | 142 | |
yusuke_kyo | 0:eb69b57ab0dd | 143 | //現在の状態を送信 |
yusuke_kyo | 0:eb69b57ab0dd | 144 | |
yusuke_kyo | 0:eb69b57ab0dd | 145 | //JoyStickの値(目標角度・リセット)を取得 |
yusuke_kyo | 0:eb69b57ab0dd | 146 | |
yusuke_kyo | 0:eb69b57ab0dd | 147 | //目標角速度を算出&送信 |
yusuke_kyo | 0:eb69b57ab0dd | 148 | myled2=1; |
yusuke_kyo | 0:eb69b57ab0dd | 149 | linear.SetGoal(linear_goal); |
yusuke_kyo | 0:eb69b57ab0dd | 150 | pitch.SetGoal(pitch_goal); |
yusuke_kyo | 0:eb69b57ab0dd | 151 | yaw.SetGoal(yaw_goal); |
yusuke_kyo | 0:eb69b57ab0dd | 152 | myled2=0; |
yusuke_kyo | 2:8b71db86f221 | 153 | /*--Dynamixel:end-------------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 154 | |
yusuke_kyo | 2:8b71db86f221 | 155 | /*--Thermal_Sensor:begin--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 156 | //値を取得 |
yusuke_kyo | 0:eb69b57ab0dd | 157 | Thermal_Sensor(); |
yusuke_kyo | 0:eb69b57ab0dd | 158 | //値を送信 |
yusuke_kyo | 0:eb69b57ab0dd | 159 | |
yusuke_kyo | 2:8b71db86f221 | 160 | /*--Thermal_Sensor:end--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 161 | |
yusuke_kyo | 2:8b71db86f221 | 162 | /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 163 | //値を取得 |
yusuke_kyo | 0:eb69b57ab0dd | 164 | CO2_data=CO2_Sensor(); |
yusuke_kyo | 0:eb69b57ab0dd | 165 | //値を送信 |
yusuke_kyo | 0:eb69b57ab0dd | 166 | |
yusuke_kyo | 2:8b71db86f221 | 167 | /*--CO2_Sensor:end--------------------------------------------------------------------------------------*/ |
yusuke_kyo | 0:eb69b57ab0dd | 168 | |
yusuke_kyo | 0:eb69b57ab0dd | 169 | //ホームポジション |
yusuke_kyo | 0:eb69b57ab0dd | 170 | if() Dyna_home_position(); |
yusuke_kyo | 0:eb69b57ab0dd | 171 | //リセット |
yusuke_kyo | 0:eb69b57ab0dd | 172 | if() Dyna_reset(); |
yusuke_kyo | 0:eb69b57ab0dd | 173 | |
yusuke_kyo | 0:eb69b57ab0dd | 174 | //終了 |
yusuke_kyo | 0:eb69b57ab0dd | 175 | if() { |
yusuke_kyo | 0:eb69b57ab0dd | 176 | Dyna_end(); |
yusuke_kyo | 0:eb69b57ab0dd | 177 | return 0; |
yusuke_kyo | 0:eb69b57ab0dd | 178 | } |
yusuke_kyo | 0:eb69b57ab0dd | 179 | } |
yusuke_kyo | 0:eb69b57ab0dd | 180 | } |