雄介 太田 / Mbed 2 deprecated YOZAKURA_ARM

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?

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 //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 }