雄介 太田 / Mbed 2 deprecated YOZAKURA_ARM

YOZAKURAのARMプログラム(ver.1)

Dependencies:   Dynamixel EthernetInterface MEMS_Thermal_Sensor mbed-rtos SerialHalfDuplex mbed

main.cpp@6:5ce16a60792b, 2015-04-02 (annotated)

Committer:
yusuke_kyo
Date:
Thu Apr 02 21:26:56 2015 +0000
Revision:
6:5ce16a60792b
Parent:
5:fa52b01a484e
Child:
7:aea2866fa06c
comment

Who changed what in which revision?

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