cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1214_essence_8_2

Dependencies:   mbed

main.cpp@5:5aa7223226df, 2019-11-22 (annotated)

Committer:
YUPPY
Date:
Fri Nov 22 05:55:36 2019 +0000
Revision:
5:5aa7223226df
Parent:
4:1354e56c7dd3
Child:
6:db1a62608047
CanSatB_Main

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YUPPY 0:6212b283430c 1 #define cansatB
YUPPY 4:1354e56c7dd3 2 #define min(x, y) ((x) < (y)) ? (x) : (y)
YUPPY 2:e2b803e3bcbc 3 #include "mbed.h" //mbed
YUPPY 2:e2b803e3bcbc 4 #include "getGPS.h" //GPS
YUPPY 2:e2b803e3bcbc 5 #include "math.h" //GPS
YUPPY 3:5d0c4b13f4e8 6 #include "TB6612.h" //motorDriver
YUPPY 4:1354e56c7dd3 7 #include "JPEGCamera.h"//カメラ
YUPPY 4:1354e56c7dd3 8 #include "base64.h"//写真ーXBee
YUPPY 2:e2b803e3bcbc 9 #include "us015.h" // 超音波センサ
YUPPY 0:6212b283430c 10 #include <stdio.h>
YUPPY 2:e2b803e3bcbc 11 US015 hs(p12,p11); //P12 :超音波センサ トリガ出力 //p11 :超音波センサ エコー入力
YUPPY 2:e2b803e3bcbc 12 GPS gps (p28,p27); //GPS
YUPPY 4:1354e56c7dd3 13
YUPPY 2:e2b803e3bcbc 14 DigitalOut FET(p21); //FET
YUPPY 4:1354e56c7dd3 15 DigitalOut Ultra(p12);
YUPPY 2:e2b803e3bcbc 16 DigitalIn thermo(p20); //焦電センサ↓
YUPPY 2:e2b803e3bcbc 17 DigitalOut led(LED1);
YUPPY 4:1354e56c7dd3 18 Serial pc(USBTX,USBRX); // tx, rx 焦電センサ↑
YUPPY 5:5aa7223226df 19 Serial xbee(p13, p14);
YUPPY 4:1354e56c7dd3 20 TB6612 left(p25,p17,p16);//motor
YUPPY 4:1354e56c7dd3 21 TB6612 right(p26,p19,p18);//motor
YUPPY 2:e2b803e3bcbc 22
YUPPY 5:5aa7223226df 23 void JPEGCamera(void);
YUPPY 5:5aa7223226df 24 void us015(void);
YUPPY 5:5aa7223226df 25 void motorFoward(void);
YUPPY 5:5aa7223226df 26 void motorFowardR(void);
YUPPY 5:5aa7223226df 27 void motorForwardL(void);
YUPPY 5:5aa7223226df 28 void motorStop(void);
YUPPY 5:5aa7223226df 29 void motorReverse(void);
YUPPY 5:5aa7223226df 30 LocalFileSystem local("local");
YUPPY 5:5aa7223226df 31
YUPPY 5:5aa7223226df 32 int main() {
YUPPY 5:5aa7223226df 33 printf("CanSat-B_Start!\r\n");
YUPPY 5:5aa7223226df 34 //FET
YUPPY 2:e2b803e3bcbc 35 FET = 0;
YUPPY 2:e2b803e3bcbc 36 SW = 1; //p21をhigh(3.3V)にする。
YUPPY 2:e2b803e3bcbc 37 while(1) {
YUPPY 2:e2b803e3bcbc 38 if(flight==1) {
YUPPY 2:e2b803e3bcbc 39 wait(10);
YUPPY 2:e2b803e3bcbc 40 }
YUPPY 2:e2b803e3bcbc 41
YUPPY 2:e2b803e3bcbc 42 else{
YUPPY 2:e2b803e3bcbc 43 if(flight==1) {
YUPPY 2:e2b803e3bcbc 44 wait(10);
YUPPY 2:e2b803e3bcbc 45 }
YUPPY 2:e2b803e3bcbc 46 else{
YUPPY 2:e2b803e3bcbc 47 FET = 0; //FET、ニクロム線切断
YUPPY 2:e2b803e3bcbc 48 wait(20);
YUPPY 2:e2b803e3bcbc 49 FET = 1;
YUPPY 2:e2b803e3bcbc 50 wait(12);
YUPPY 2:e2b803e3bcbc 51 FET = 0;
YUPPY 2:e2b803e3bcbc 52 wait(10);
YUPPY 2:e2b803e3bcbc 53 FET = 1;
YUPPY 2:e2b803e3bcbc 54 wait(12);
YUPPY 2:e2b803e3bcbc 55 FET = 0;
YUPPY 2:e2b803e3bcbc 56 SW = 0; //p21をlow(0V)にする。
YUPPY 2:e2b803e3bcbc 57 break;
YUPPY 2:e2b803e3bcbc 58 }
YUPPY 2:e2b803e3bcbc 59 }
YUPPY 2:e2b803e3bcbc 60 }
YUPPY 5:5aa7223226df 61
YUPPY 5:5aa7223226df 62 //以下GPS
YUPPY 2:e2b803e3bcbc 63 double a;
YUPPY 2:e2b803e3bcbc 64 double b;
YUPPY 2:e2b803e3bcbc 65 double distance;
YUPPY 2:e2b803e3bcbc 66
YUPPY 2:e2b803e3bcbc 67 pc.printf("GPS Start\r\n");
YUPPY 2:e2b803e3bcbc 68
YUPPY 2:e2b803e3bcbc 69 while(1) {
YUPPY 2:e2b803e3bcbc 70 if(gps.getgps()){
YUPPY 2:e2b803e3bcbc 71 a = gps.latitude;
YUPPY 2:e2b803e3bcbc 72 b = gps.longitude;
YUPPY 2:e2b803e3bcbc 73
YUPPY 2:e2b803e3bcbc 74 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
YUPPY 2:e2b803e3bcbc 75
YUPPY 2:e2b803e3bcbc 76 break;
YUPPY 2:e2b803e3bcbc 77
YUPPY 2:e2b803e3bcbc 78 }else{
YUPPY 2:e2b803e3bcbc 79 pc.printf("NO DATA\r\n");//データ取得失敗
YUPPY 2:e2b803e3bcbc 80 wait(1);
YUPPY 2:e2b803e3bcbc 81 }
YUPPY 2:e2b803e3bcbc 82 }
YUPPY 2:e2b803e3bcbc 83 while(1){
YUPPY 2:e2b803e3bcbc 84 if(gps.getgps()) {
YUPPY 2:e2b803e3bcbc 85
YUPPY 2:e2b803e3bcbc 86 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
YUPPY 2:e2b803e3bcbc 87
YUPPY 2:e2b803e3bcbc 88 // 球面三角法により、大円距離(メートル)を求める
YUPPY 2:e2b803e3bcbc 89 double c;
YUPPY 2:e2b803e3bcbc 90 double d;
YUPPY 2:e2b803e3bcbc 91 c = gps.latitude;
YUPPY 2:e2b803e3bcbc 92 d = gps.longitude;
YUPPY 2:e2b803e3bcbc 93
YUPPY 2:e2b803e3bcbc 94 const double pi = 3.14159265359; // 円周率
YUPPY 2:e2b803e3bcbc 95
YUPPY 2:e2b803e3bcbc 96 double ra = a * pi / 180;
YUPPY 2:e2b803e3bcbc 97 double rb = b * pi / 180; // 緯度経度をラジアンに変換
YUPPY 2:e2b803e3bcbc 98 double rc = c * pi / 180;
YUPPY 2:e2b803e3bcbc 99 double rd = d * pi / 180;
YUPPY 2:e2b803e3bcbc 100
YUPPY 2:e2b803e3bcbc 101 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
YUPPY 2:e2b803e3bcbc 102 double rr = acos(e);
YUPPY 2:e2b803e3bcbc 103
YUPPY 2:e2b803e3bcbc 104 const double earth_radius = 6378140; // 地球赤道半径(m)
YUPPY 2:e2b803e3bcbc 105
YUPPY 2:e2b803e3bcbc 106 distance = earth_radius * rr; // 2点間の距離(m)
YUPPY 2:e2b803e3bcbc 107
YUPPY 2:e2b803e3bcbc 108
YUPPY 2:e2b803e3bcbc 109
YUPPY 2:e2b803e3bcbc 110
YUPPY 2:e2b803e3bcbc 111 if (distance<5){
YUPPY 2:e2b803e3bcbc 112 printf("%lf\r\n",distance);
YUPPY 5:5aa7223226df 113 motorFoward();
YUPPY 2:e2b803e3bcbc 114 }else{
YUPPY 5:5aa7223226df 115 motorStop();
YUPPY 2:e2b803e3bcbc 116 pc.printf("5m clear!");
YUPPY 2:e2b803e3bcbc 117 break;
YUPPY 2:e2b803e3bcbc 118 }
YUPPY 2:e2b803e3bcbc 119
YUPPY 2:e2b803e3bcbc 120 }else{
YUPPY 2:e2b803e3bcbc 121 pc.printf("NO DATA\r\n");//データ取得失敗
YUPPY 2:e2b803e3bcbc 122 wait(1);
YUPPY 2:e2b803e3bcbc 123 }
YUPPY 2:e2b803e3bcbc 124 }
YUPPY 5:5aa7223226df 125 //GPS End
YUPPY 5:5aa7223226df 126
YUPPY 4:1354e56c7dd3 127 float th;
YUPPY 4:1354e56c7dd3 128 Timer tm;
YUPPY 4:1354e56c7dd3 129 pc.printf("start\r\n");
YUPPY 4:1354e56c7dd3 130
YUPPY 4:1354e56c7dd3 131 bool detected=false;
YUPPY 4:1354e56c7dd3 132 thermo=0; //焦電off
YUPPY 4:1354e56c7dd3 133 Ultra=1;//超音波on
YUPPY 4:1354e56c7dd3 134
YUPPY 5:5aa7223226df 135 us015();
YUPPY 4:1354e56c7dd3 136 if(distance<2000){//超音波反応
YUPPY 4:1354e56c7dd3 137 Ultra=0;//超音波off
YUPPY 4:1354e56c7dd3 138 thermo=1;//焦電on
YUPPY 5:5aa7223226df 139 motorStop();
YUPPY 5:5aa7223226df 140
YUPPY 4:1354e56c7dd3 141 if(true)
YUPPY 4:1354e56c7dd3 142 th = thermo;
YUPPY 4:1354e56c7dd3 143 if(th=1 && !detected) {//焦電反応ありの場合
YUPPY 4:1354e56c7dd3 144 detected=true;
YUPPY 4:1354e56c7dd3 145 pc.printf("human\r\n");
YUPPY 4:1354e56c7dd3 146 tm.reset();
YUPPY 4:1354e56c7dd3 147 tm.start();
YUPPY 4:1354e56c7dd3 148 thermo=0;
YUPPY 5:5aa7223226df 149 JPEGCamera();
YUPPY 5:5aa7223226df 150
YUPPY 5:5aa7223226df 151 FILE *fp;
YUPPY 5:5aa7223226df 152 base64 *bs;
YUPPY 5:5aa7223226df 153 int c;
YUPPY 5:5aa7223226df 154
YUPPY 5:5aa7223226df 155 xbee.printf("charizard!!!\r\n");
YUPPY 5:5aa7223226df 156 bs = new base64();
YUPPY 5:5aa7223226df 157 bs->Encode("/local/PICT000.jpg","/local/d.txt");
YUPPY 5:5aa7223226df 158
YUPPY 5:5aa7223226df 159
YUPPY 5:5aa7223226df 160 if((fp=fopen("/local/d.txt","r"))!=NULL)
YUPPY 5:5aa7223226df 161 {
YUPPY 5:5aa7223226df 162 while ((c=fgetc(fp))!=EOF){
YUPPY 5:5aa7223226df 163 xbee.printf("%c",c);
YUPPY 5:5aa7223226df 164 }
YUPPY 5:5aa7223226df 165 fclose(fp);
YUPPY 5:5aa7223226df 166 }
YUPPY 5:5aa7223226df 167
YUPPY 5:5aa7223226df 168
YUPPY 5:5aa7223226df 169 }else{//焦電反応なしの場合
YUPPY 5:5aa7223226df 170 detected=false;
YUPPY 5:5aa7223226df 171 thermo=0;
YUPPY 5:5aa7223226df 172 Ultra=1;
YUPPY 5:5aa7223226df 173 }
YUPPY 5:5aa7223226df 174
YUPPY 5:5aa7223226df 175
YUPPY 5:5aa7223226df 176 //while(true)
YUPPY 5:5aa7223226df 177
YUPPY 5:5aa7223226df 178
YUPPY 5:5aa7223226df 179 //超音波センサー反応あり
YUPPY 5:5aa7223226df 180 //停止
YUPPY 5:5aa7223226df 181 //超音波センサーOFF
YUPPY 5:5aa7223226df 182 //焦電センサーON
YUPPY 5:5aa7223226df 183
YUPPY 5:5aa7223226df 184 //焦電センサー反応あり
YUPPY 5:5aa7223226df 185 //焦電センサーOFF
YUPPY 5:5aa7223226df 186 //カメラ起動
YUPPY 5:5aa7223226df 187 //カメラOFF
YUPPY 5:5aa7223226df 188 //XBeeによりPCへ送信
YUPPY 5:5aa7223226df 189 if(receiveOK()=true){
YUPPY 5:5aa7223226df 190 printf("Unknown Creature has been descovered!\n");
YUPPY 5:5aa7223226df 191 }
YUPPY 5:5aa7223226df 192 //"OK"受信、ミッション終了
YUPPY 5:5aa7223226df 193 else if(receiveOK()=false){
YUPPY 5:5aa7223226df 194 printf("Continue!\n");
YUPPY 5:5aa7223226df 195 }
YUPPY 5:5aa7223226df 196 //"NO"受信、ミッション再開
YUPPY 5:5aa7223226df 197
YUPPY 5:5aa7223226df 198 else if(detected=false){
YUPPY 5:5aa7223226df 199 thermo=0;
YUPPY 5:5aa7223226df 200 motorFowardR();
YUPPY 5:5aa7223226df 201 wait(3);
YUPPY 5:5aa7223226df 202 Ultra=1;
YUPPY 5:5aa7223226df 203 motorFoward();
YUPPY 5:5aa7223226df 204 }
YUPPY 5:5aa7223226df 205
YUPPY 5:5aa7223226df 206 //焦電センサー反応無し
YUPPY 5:5aa7223226df 207 //焦電センサーOFF
YUPPY 5:5aa7223226df 208 //方向転換
YUPPY 5:5aa7223226df 209 //超音波センサーON
YUPPY 5:5aa7223226df 210 //直進
YUPPY 5:5aa7223226df 211 }
YUPPY 5:5aa7223226df 212 else if(distance>=2000){
YUPPY 5:5aa7223226df 213 motorStop();
YUPPY 5:5aa7223226df 214 wait(2);
YUPPY 5:5aa7223226df 215 motorFowardR();
YUPPY 5:5aa7223226df 216 wait(3);
YUPPY 5:5aa7223226df 217 }
YUPPY 5:5aa7223226df 218 //超音波センサー反応無し
YUPPY 5:5aa7223226df 219 //停止
YUPPY 5:5aa7223226df 220 //方向転換
YUPPY 5:5aa7223226df 221 //直進
YUPPY 5:5aa7223226df 222 }
YUPPY 5:5aa7223226df 223
YUPPY 5:5aa7223226df 224
YUPPY 5:5aa7223226df 225 void JPEGCamera(){
YUPPY 5:5aa7223226df 226 const int RESPONSE_TIMEOUT = 500;
YUPPY 4:1354e56c7dd3 227 const int DATA_TIMEOUT = 1000;
YUPPY 4:1354e56c7dd3 228
YUPPY 4:1354e56c7dd3 229 JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
YUPPY 4:1354e56c7dd3 230 printf("AA\r\n");
YUPPY 4:1354e56c7dd3 231 baud(38400);
YUPPY 4:1354e56c7dd3 232 state = READY;
YUPPY 4:1354e56c7dd3 233 }
YUPPY 4:1354e56c7dd3 234
YUPPY 4:1354e56c7dd3 235 bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
YUPPY 4:1354e56c7dd3 236 char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
YUPPY 4:1354e56c7dd3 237 int ret = sendReceive(buf, sizeof buf, 5);
YUPPY 4:1354e56c7dd3 238
YUPPY 4:1354e56c7dd3 239 if (ret == 5 && buf[0] == 0x76) {
YUPPY 4:1354e56c7dd3 240 if (doReset)
YUPPY 4:1354e56c7dd3 241 reset();
YUPPY 4:1354e56c7dd3 242 return true;
YUPPY 4:1354e56c7dd3 243 } else
YUPPY 4:1354e56c7dd3 244 return false;
YUPPY 4:1354e56c7dd3 245 }
YUPPY 4:1354e56c7dd3 246
YUPPY 4:1354e56c7dd3 247 bool JPEGCamera::isReady() {
YUPPY 4:1354e56c7dd3 248 return state == READY;
YUPPY 4:1354e56c7dd3 249 }
YUPPY 4:1354e56c7dd3 250
YUPPY 4:1354e56c7dd3 251 bool JPEGCamera::isProcessing() {
YUPPY 4:1354e56c7dd3 252 return state == PROCESSING;
YUPPY 4:1354e56c7dd3 253 }
YUPPY 4:1354e56c7dd3 254
YUPPY 4:1354e56c7dd3 255 bool JPEGCamera::takePicture(char *filename) {
YUPPY 4:1354e56c7dd3 256 if (state == READY) {
YUPPY 4:1354e56c7dd3 257 fp = fopen(filename, "wb");
YUPPY 4:1354e56c7dd3 258 if (fp != 0) {
YUPPY 4:1354e56c7dd3 259 if (takePicture()) {
YUPPY 4:1354e56c7dd3 260 imageSize = getImageSize();
YUPPY 4:1354e56c7dd3 261 address = 0;
YUPPY 4:1354e56c7dd3 262 state = PROCESSING;
YUPPY 4:1354e56c7dd3 263 } else {
YUPPY 4:1354e56c7dd3 264 fclose(fp);
YUPPY 4:1354e56c7dd3 265 printf("takePicture(%s) failed", filename);
YUPPY 4:1354e56c7dd3 266 state = ERROR;
YUPPY 4:1354e56c7dd3 267 }
YUPPY 4:1354e56c7dd3 268 } else {
YUPPY 4:1354e56c7dd3 269 printf("fopen() failed");
YUPPY 4:1354e56c7dd3 270 state = ERROR;
YUPPY 4:1354e56c7dd3 271 }
YUPPY 3:5d0c4b13f4e8 272 }
YUPPY 4:1354e56c7dd3 273 return state != ERROR;
YUPPY 4:1354e56c7dd3 274 }
YUPPY 4:1354e56c7dd3 275
YUPPY 4:1354e56c7dd3 276 bool JPEGCamera::processPicture() {
YUPPY 4:1354e56c7dd3 277 if (state == PROCESSING) {
YUPPY 4:1354e56c7dd3 278 if (address < imageSize) {
YUPPY 4:1354e56c7dd3 279 char data[1024];
YUPPY 4:1354e56c7dd3 280 int size = readData(data, min(sizeof(data), imageSize - address), address);
YUPPY 4:1354e56c7dd3 281 int ret = fwrite(data, size, 1, fp);
YUPPY 4:1354e56c7dd3 282 if (ret > 0)
YUPPY 4:1354e56c7dd3 283 address += size;
YUPPY 4:1354e56c7dd3 284 if (ret == 0 || address >= imageSize) {
YUPPY 4:1354e56c7dd3 285 stopPictures();
YUPPY 4:1354e56c7dd3 286 fclose(fp);
YUPPY 4:1354e56c7dd3 287 wait(0.1); // ????
YUPPY 4:1354e56c7dd3 288 state = ret > 0 ? READY : ERROR;
YUPPY 4:1354e56c7dd3 289 }
YUPPY 4:1354e56c7dd3 290 }
YUPPY 4:1354e56c7dd3 291 }
YUPPY 4:1354e56c7dd3 292
YUPPY 4:1354e56c7dd3 293 return state == PROCESSING || state == READY;
YUPPY 3:5d0c4b13f4e8 294 }
YUPPY 0:6212b283430c 295
YUPPY 4:1354e56c7dd3 296 bool JPEGCamera::reset() {
YUPPY 4:1354e56c7dd3 297 char buf[4] = {0x56, 0x00, 0x26, 0x00};
YUPPY 4:1354e56c7dd3 298 int ret = sendReceive(buf, sizeof buf, 4);
YUPPY 4:1354e56c7dd3 299 if (ret == 4 && buf[0] == 0x76) {
YUPPY 4:1354e56c7dd3 300 wait(4.0);
YUPPY 4:1354e56c7dd3 301 state = READY;
YUPPY 4:1354e56c7dd3 302 } else {
YUPPY 4:1354e56c7dd3 303 state = ERROR;
YUPPY 4:1354e56c7dd3 304 }
YUPPY 4:1354e56c7dd3 305 return state == READY;
YUPPY 4:1354e56c7dd3 306 }
YUPPY 4:1354e56c7dd3 307
YUPPY 4:1354e56c7dd3 308 bool JPEGCamera::takePicture() {
YUPPY 4:1354e56c7dd3 309 char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
YUPPY 4:1354e56c7dd3 310 int ret = sendReceive(buf, sizeof buf, 5);
YUPPY 4:1354e56c7dd3 311
YUPPY 4:1354e56c7dd3 312 return ret == 5 && buf[0] == 0x76;
YUPPY 4:1354e56c7dd3 313 }
YUPPY 4:1354e56c7dd3 314
YUPPY 4:1354e56c7dd3 315 bool JPEGCamera::stopPictures() {
YUPPY 4:1354e56c7dd3 316 char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
YUPPY 4:1354e56c7dd3 317 int ret = sendReceive(buf, sizeof buf, 5);
YUPPY 4:1354e56c7dd3 318
YUPPY 4:1354e56c7dd3 319 return ret == 4 && buf[0] == 0x76;
YUPPY 4:1354e56c7dd3 320 }
YUPPY 4:1354e56c7dd3 321
YUPPY 4:1354e56c7dd3 322 int JPEGCamera::getImageSize() {
YUPPY 4:1354e56c7dd3 323 char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
YUPPY 4:1354e56c7dd3 324 int ret = sendReceive(buf, sizeof buf, 9);
YUPPY 4:1354e56c7dd3 325
YUPPY 4:1354e56c7dd3 326 //The size is in the last 2 characters of the response.
YUPPY 4:1354e56c7dd3 327 return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
YUPPY 4:1354e56c7dd3 328 }
YUPPY 4:1354e56c7dd3 329
YUPPY 4:1354e56c7dd3 330 int JPEGCamera::readData(char *dataBuf, int size, int address) {
YUPPY 4:1354e56c7dd3 331 char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
YUPPY 4:1354e56c7dd3 332 address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
YUPPY 4:1354e56c7dd3 333 };
YUPPY 4:1354e56c7dd3 334 int ret = sendReceive(buf, sizeof buf, 5);
YUPPY 4:1354e56c7dd3 335
YUPPY 4:1354e56c7dd3 336 return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
YUPPY 4:1354e56c7dd3 337 }
YUPPY 4:1354e56c7dd3 338
YUPPY 4:1354e56c7dd3 339 int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
YUPPY 4:1354e56c7dd3 340 while (readable()) getc();
YUPPY 4:1354e56c7dd3 341
YUPPY 4:1354e56c7dd3 342 for (int i = 0; i < sendSize; i++) putc(buf[i]);
YUPPY 4:1354e56c7dd3 343
YUPPY 4:1354e56c7dd3 344 return receive(buf, receiveSize, RESPONSE_TIMEOUT);
YUPPY 4:1354e56c7dd3 345 }
YUPPY 4:1354e56c7dd3 346
YUPPY 4:1354e56c7dd3 347 int JPEGCamera::receive(char *buf, int size, int timeout) {
YUPPY 4:1354e56c7dd3 348 timer.start();
YUPPY 4:1354e56c7dd3 349 timer.reset();
YUPPY 4:1354e56c7dd3 350
YUPPY 4:1354e56c7dd3 351 int i = 0;
YUPPY 4:1354e56c7dd3 352 while (i < size && timer.read_ms() < timeout) {
YUPPY 4:1354e56c7dd3 353 if (readable())
YUPPY 4:1354e56c7dd3 354 buf[i++] = getc();
YUPPY 4:1354e56c7dd3 355 }
YUPPY 4:1354e56c7dd3 356
YUPPY 4:1354e56c7dd3 357 return i;
YUPPY 4:1354e56c7dd3 358 }
YUPPY 5:5aa7223226df 359 void us015(){
YUPPY 5:5aa7223226df 360 hs.TrigerOut();
YUPPY 5:5aa7223226df 361 wait(1);
YUPPY 5:5aa7223226df 362 int distance;
YUPPY 5:5aa7223226df 363 distance = hs.GetDistance();
YUPPY 5:5aa7223226df 364 printf("%d\r\n",distance);//距離出力
YUPPY 5:5aa7223226df 365 }
YUPPY 4:1354e56c7dd3 366
YUPPY 5:5aa7223226df 367 void motorFoward(){
YUPPY 5:5aa7223226df 368 left = 100;
YUPPY 5:5aa7223226df 369 right = 100;
YUPPY 5:5aa7223226df 370 printf("直進\n\r");
YUPPY 5:5aa7223226df 371 }
YUPPY 5:5aa7223226df 372 void motorFowardR(){
YUPPY 5:5aa7223226df 373 left = 100;
YUPPY 5:5aa7223226df 374 right = 20;
YUPPY 5:5aa7223226df 375 printf("右折\n\r");
YUPPY 5:5aa7223226df 376 }
YUPPY 5:5aa7223226df 377 void motorForwardL(){
YUPPY 5:5aa7223226df 378 left = 20;
YUPPY 5:5aa7223226df 379 right = 100;
YUPPY 5:5aa7223226df 380 printf("左折\n\r");}
YUPPY 5:5aa7223226df 381 void motorStop(){
YUPPY 5:5aa7223226df 382 left = 0;
YUPPY 5:5aa7223226df 383 right = 0;
YUPPY 5:5aa7223226df 384 printf("停止\n\r");}
YUPPY 5:5aa7223226df 385 void motorReverse(){
YUPPY 5:5aa7223226df 386 left = -50;
YUPPY 5:5aa7223226df 387 right = -50;
YUPPY 5:5aa7223226df 388 printf("バック\n\r");
YUPPY 5:5aa7223226df 389 }