mainまでいけました カメラのとこからコンパイルできません
Dependencies: mbed CameraUS015sb612-3
main.cpp@6:db1a62608047, 2019-11-22 (annotated)
- Committer:
- YUPPY
- Date:
- Fri Nov 22 08:52:55 2019 +0000
- Revision:
- 6:db1a62608047
- Parent:
- 5:5aa7223226df
yeah!
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:db1a62608047 | 16 | DigitalOut 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 | |
YUPPY | 6:db1a62608047 | 35 | printf("FETのコピーはここ"); |
YUPPY | 5:5aa7223226df | 36 | //以下GPS |
YUPPY | 2:e2b803e3bcbc | 37 | double a; |
YUPPY | 2:e2b803e3bcbc | 38 | double b; |
YUPPY | 2:e2b803e3bcbc | 39 | double distance; |
YUPPY | 2:e2b803e3bcbc | 40 | |
YUPPY | 2:e2b803e3bcbc | 41 | pc.printf("GPS Start\r\n"); |
YUPPY | 2:e2b803e3bcbc | 42 | |
YUPPY | 2:e2b803e3bcbc | 43 | while(1) { |
YUPPY | 2:e2b803e3bcbc | 44 | if(gps.getgps()){ |
YUPPY | 2:e2b803e3bcbc | 45 | a = gps.latitude; |
YUPPY | 2:e2b803e3bcbc | 46 | b = gps.longitude; |
YUPPY | 2:e2b803e3bcbc | 47 | |
YUPPY | 2:e2b803e3bcbc | 48 | pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 |
YUPPY | 2:e2b803e3bcbc | 49 | |
YUPPY | 2:e2b803e3bcbc | 50 | break; |
YUPPY | 2:e2b803e3bcbc | 51 | |
YUPPY | 2:e2b803e3bcbc | 52 | }else{ |
YUPPY | 2:e2b803e3bcbc | 53 | pc.printf("NO DATA\r\n");//データ取得失敗 |
YUPPY | 2:e2b803e3bcbc | 54 | wait(1); |
YUPPY | 2:e2b803e3bcbc | 55 | } |
YUPPY | 2:e2b803e3bcbc | 56 | } |
YUPPY | 2:e2b803e3bcbc | 57 | while(1){ |
YUPPY | 2:e2b803e3bcbc | 58 | if(gps.getgps()) { |
YUPPY | 2:e2b803e3bcbc | 59 | |
YUPPY | 2:e2b803e3bcbc | 60 | pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 |
YUPPY | 2:e2b803e3bcbc | 61 | |
YUPPY | 2:e2b803e3bcbc | 62 | // 球面三角法により、大円距離(メートル)を求める |
YUPPY | 2:e2b803e3bcbc | 63 | double c; |
YUPPY | 2:e2b803e3bcbc | 64 | double d; |
YUPPY | 2:e2b803e3bcbc | 65 | c = gps.latitude; |
YUPPY | 2:e2b803e3bcbc | 66 | d = gps.longitude; |
YUPPY | 2:e2b803e3bcbc | 67 | |
YUPPY | 2:e2b803e3bcbc | 68 | const double pi = 3.14159265359; // 円周率 |
YUPPY | 2:e2b803e3bcbc | 69 | |
YUPPY | 2:e2b803e3bcbc | 70 | double ra = a * pi / 180; |
YUPPY | 2:e2b803e3bcbc | 71 | double rb = b * pi / 180; // 緯度経度をラジアンに変換 |
YUPPY | 2:e2b803e3bcbc | 72 | double rc = c * pi / 180; |
YUPPY | 2:e2b803e3bcbc | 73 | double rd = d * pi / 180; |
YUPPY | 2:e2b803e3bcbc | 74 | |
YUPPY | 2:e2b803e3bcbc | 75 | double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める |
YUPPY | 2:e2b803e3bcbc | 76 | double rr = acos(e); |
YUPPY | 2:e2b803e3bcbc | 77 | |
YUPPY | 2:e2b803e3bcbc | 78 | const double earth_radius = 6378140; // 地球赤道半径(m) |
YUPPY | 2:e2b803e3bcbc | 79 | |
YUPPY | 2:e2b803e3bcbc | 80 | distance = earth_radius * rr; // 2点間の距離(m) |
YUPPY | 2:e2b803e3bcbc | 81 | |
YUPPY | 2:e2b803e3bcbc | 82 | |
YUPPY | 2:e2b803e3bcbc | 83 | |
YUPPY | 2:e2b803e3bcbc | 84 | |
YUPPY | 2:e2b803e3bcbc | 85 | if (distance<5){ |
YUPPY | 2:e2b803e3bcbc | 86 | printf("%lf\r\n",distance); |
YUPPY | 5:5aa7223226df | 87 | motorFoward(); |
YUPPY | 2:e2b803e3bcbc | 88 | }else{ |
YUPPY | 5:5aa7223226df | 89 | motorStop(); |
YUPPY | 2:e2b803e3bcbc | 90 | pc.printf("5m clear!"); |
YUPPY | 2:e2b803e3bcbc | 91 | break; |
YUPPY | 2:e2b803e3bcbc | 92 | } |
YUPPY | 2:e2b803e3bcbc | 93 | |
YUPPY | 2:e2b803e3bcbc | 94 | }else{ |
YUPPY | 2:e2b803e3bcbc | 95 | pc.printf("NO DATA\r\n");//データ取得失敗 |
YUPPY | 2:e2b803e3bcbc | 96 | wait(1); |
YUPPY | 2:e2b803e3bcbc | 97 | } |
YUPPY | 2:e2b803e3bcbc | 98 | } |
YUPPY | 5:5aa7223226df | 99 | //GPS End |
YUPPY | 5:5aa7223226df | 100 | |
YUPPY | 4:1354e56c7dd3 | 101 | float th; |
YUPPY | 4:1354e56c7dd3 | 102 | Timer tm; |
YUPPY | 4:1354e56c7dd3 | 103 | pc.printf("start\r\n"); |
YUPPY | 6:db1a62608047 | 104 | |
YUPPY | 4:1354e56c7dd3 | 105 | bool detected=false; |
YUPPY | 4:1354e56c7dd3 | 106 | thermo=0; //焦電off |
YUPPY | 4:1354e56c7dd3 | 107 | Ultra=1;//超音波on |
YUPPY | 6:db1a62608047 | 108 | |
YUPPY | 5:5aa7223226df | 109 | us015(); |
YUPPY | 4:1354e56c7dd3 | 110 | if(distance<2000){//超音波反応 |
YUPPY | 4:1354e56c7dd3 | 111 | Ultra=0;//超音波off |
YUPPY | 4:1354e56c7dd3 | 112 | thermo=1;//焦電on |
YUPPY | 5:5aa7223226df | 113 | motorStop(); |
YUPPY | 5:5aa7223226df | 114 | |
YUPPY | 4:1354e56c7dd3 | 115 | if(true) |
YUPPY | 4:1354e56c7dd3 | 116 | th = thermo; |
YUPPY | 4:1354e56c7dd3 | 117 | if(th=1 && !detected) {//焦電反応ありの場合 |
YUPPY | 4:1354e56c7dd3 | 118 | detected=true; |
YUPPY | 4:1354e56c7dd3 | 119 | pc.printf("human\r\n"); |
YUPPY | 4:1354e56c7dd3 | 120 | tm.reset(); |
YUPPY | 4:1354e56c7dd3 | 121 | tm.start(); |
YUPPY | 4:1354e56c7dd3 | 122 | thermo=0; |
YUPPY | 5:5aa7223226df | 123 | JPEGCamera(); |
YUPPY | 5:5aa7223226df | 124 | |
YUPPY | 5:5aa7223226df | 125 | FILE *fp; |
YUPPY | 5:5aa7223226df | 126 | base64 *bs; |
YUPPY | 5:5aa7223226df | 127 | int c; |
YUPPY | 5:5aa7223226df | 128 | |
YUPPY | 5:5aa7223226df | 129 | xbee.printf("charizard!!!\r\n"); |
YUPPY | 5:5aa7223226df | 130 | bs = new base64(); |
YUPPY | 5:5aa7223226df | 131 | bs->Encode("/local/PICT000.jpg","/local/d.txt"); |
YUPPY | 5:5aa7223226df | 132 | |
YUPPY | 5:5aa7223226df | 133 | |
YUPPY | 5:5aa7223226df | 134 | if((fp=fopen("/local/d.txt","r"))!=NULL) |
YUPPY | 5:5aa7223226df | 135 | { |
YUPPY | 5:5aa7223226df | 136 | while ((c=fgetc(fp))!=EOF){ |
YUPPY | 5:5aa7223226df | 137 | xbee.printf("%c",c); |
YUPPY | 5:5aa7223226df | 138 | } |
YUPPY | 5:5aa7223226df | 139 | fclose(fp); |
YUPPY | 5:5aa7223226df | 140 | } |
YUPPY | 5:5aa7223226df | 141 | |
YUPPY | 5:5aa7223226df | 142 | |
YUPPY | 5:5aa7223226df | 143 | }else{//焦電反応なしの場合 |
YUPPY | 5:5aa7223226df | 144 | detected=false; |
YUPPY | 5:5aa7223226df | 145 | thermo=0; |
YUPPY | 5:5aa7223226df | 146 | Ultra=1; |
YUPPY | 5:5aa7223226df | 147 | } |
YUPPY | 5:5aa7223226df | 148 | |
YUPPY | 5:5aa7223226df | 149 | |
YUPPY | 5:5aa7223226df | 150 | //while(true) |
YUPPY | 5:5aa7223226df | 151 | |
YUPPY | 5:5aa7223226df | 152 | |
YUPPY | 5:5aa7223226df | 153 | //超音波センサー反応あり |
YUPPY | 5:5aa7223226df | 154 | //停止 |
YUPPY | 5:5aa7223226df | 155 | //超音波センサーOFF |
YUPPY | 5:5aa7223226df | 156 | //焦電センサーON |
YUPPY | 5:5aa7223226df | 157 | |
YUPPY | 5:5aa7223226df | 158 | //焦電センサー反応あり |
YUPPY | 5:5aa7223226df | 159 | //焦電センサーOFF |
YUPPY | 5:5aa7223226df | 160 | //カメラ起動 |
YUPPY | 5:5aa7223226df | 161 | //カメラOFF |
YUPPY | 5:5aa7223226df | 162 | //XBeeによりPCへ送信 |
YUPPY | 6:db1a62608047 | 163 | printf("OK1はここに\r\n"); |
YUPPY | 5:5aa7223226df | 164 | //"OK"受信、ミッション終了 |
YUPPY | 6:db1a62608047 | 165 | printf("OK2はここに\r\n"); |
YUPPY | 5:5aa7223226df | 166 | //"NO"受信、ミッション再開 |
YUPPY | 5:5aa7223226df | 167 | |
YUPPY | 6:db1a62608047 | 168 | printf("NOはここに"); |
YUPPY | 5:5aa7223226df | 169 | |
YUPPY | 5:5aa7223226df | 170 | //焦電センサー反応無し |
YUPPY | 5:5aa7223226df | 171 | //焦電センサーOFF |
YUPPY | 5:5aa7223226df | 172 | //方向転換 |
YUPPY | 5:5aa7223226df | 173 | //超音波センサーON |
YUPPY | 5:5aa7223226df | 174 | //直進 |
YUPPY | 5:5aa7223226df | 175 | } |
YUPPY | 5:5aa7223226df | 176 | else if(distance>=2000){ |
YUPPY | 5:5aa7223226df | 177 | motorStop(); |
YUPPY | 5:5aa7223226df | 178 | wait(2); |
YUPPY | 5:5aa7223226df | 179 | motorFowardR(); |
YUPPY | 5:5aa7223226df | 180 | wait(3); |
YUPPY | 5:5aa7223226df | 181 | } |
YUPPY | 5:5aa7223226df | 182 | //超音波センサー反応無し |
YUPPY | 5:5aa7223226df | 183 | //停止 |
YUPPY | 5:5aa7223226df | 184 | //方向転換 |
YUPPY | 5:5aa7223226df | 185 | //直進 |
YUPPY | 5:5aa7223226df | 186 | } |
YUPPY | 5:5aa7223226df | 187 | |
YUPPY | 5:5aa7223226df | 188 | |
YUPPY | 5:5aa7223226df | 189 | void JPEGCamera(){ |
YUPPY | 5:5aa7223226df | 190 | const int RESPONSE_TIMEOUT = 500; |
YUPPY | 4:1354e56c7dd3 | 191 | const int DATA_TIMEOUT = 1000; |
YUPPY | 4:1354e56c7dd3 | 192 | |
YUPPY | 4:1354e56c7dd3 | 193 | JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) { |
YUPPY | 4:1354e56c7dd3 | 194 | printf("AA\r\n"); |
YUPPY | 4:1354e56c7dd3 | 195 | baud(38400); |
YUPPY | 4:1354e56c7dd3 | 196 | state = READY; |
YUPPY | 4:1354e56c7dd3 | 197 | } |
YUPPY | 4:1354e56c7dd3 | 198 | |
YUPPY | 4:1354e56c7dd3 | 199 | bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) { |
YUPPY | 4:1354e56c7dd3 | 200 | char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size}; |
YUPPY | 4:1354e56c7dd3 | 201 | int ret = sendReceive(buf, sizeof buf, 5); |
YUPPY | 4:1354e56c7dd3 | 202 | |
YUPPY | 4:1354e56c7dd3 | 203 | if (ret == 5 && buf[0] == 0x76) { |
YUPPY | 4:1354e56c7dd3 | 204 | if (doReset) |
YUPPY | 4:1354e56c7dd3 | 205 | reset(); |
YUPPY | 4:1354e56c7dd3 | 206 | return true; |
YUPPY | 4:1354e56c7dd3 | 207 | } else |
YUPPY | 4:1354e56c7dd3 | 208 | return false; |
YUPPY | 4:1354e56c7dd3 | 209 | } |
YUPPY | 4:1354e56c7dd3 | 210 | |
YUPPY | 4:1354e56c7dd3 | 211 | bool JPEGCamera::isReady() { |
YUPPY | 4:1354e56c7dd3 | 212 | return state == READY; |
YUPPY | 4:1354e56c7dd3 | 213 | } |
YUPPY | 4:1354e56c7dd3 | 214 | |
YUPPY | 4:1354e56c7dd3 | 215 | bool JPEGCamera::isProcessing() { |
YUPPY | 4:1354e56c7dd3 | 216 | return state == PROCESSING; |
YUPPY | 4:1354e56c7dd3 | 217 | } |
YUPPY | 4:1354e56c7dd3 | 218 | |
YUPPY | 4:1354e56c7dd3 | 219 | bool JPEGCamera::takePicture(char *filename) { |
YUPPY | 4:1354e56c7dd3 | 220 | if (state == READY) { |
YUPPY | 4:1354e56c7dd3 | 221 | fp = fopen(filename, "wb"); |
YUPPY | 4:1354e56c7dd3 | 222 | if (fp != 0) { |
YUPPY | 4:1354e56c7dd3 | 223 | if (takePicture()) { |
YUPPY | 4:1354e56c7dd3 | 224 | imageSize = getImageSize(); |
YUPPY | 4:1354e56c7dd3 | 225 | address = 0; |
YUPPY | 4:1354e56c7dd3 | 226 | state = PROCESSING; |
YUPPY | 4:1354e56c7dd3 | 227 | } else { |
YUPPY | 4:1354e56c7dd3 | 228 | fclose(fp); |
YUPPY | 4:1354e56c7dd3 | 229 | printf("takePicture(%s) failed", filename); |
YUPPY | 4:1354e56c7dd3 | 230 | state = ERROR; |
YUPPY | 4:1354e56c7dd3 | 231 | } |
YUPPY | 4:1354e56c7dd3 | 232 | } else { |
YUPPY | 4:1354e56c7dd3 | 233 | printf("fopen() failed"); |
YUPPY | 4:1354e56c7dd3 | 234 | state = ERROR; |
YUPPY | 4:1354e56c7dd3 | 235 | } |
YUPPY | 3:5d0c4b13f4e8 | 236 | } |
YUPPY | 4:1354e56c7dd3 | 237 | return state != ERROR; |
YUPPY | 4:1354e56c7dd3 | 238 | } |
YUPPY | 4:1354e56c7dd3 | 239 | |
YUPPY | 4:1354e56c7dd3 | 240 | bool JPEGCamera::processPicture() { |
YUPPY | 4:1354e56c7dd3 | 241 | if (state == PROCESSING) { |
YUPPY | 4:1354e56c7dd3 | 242 | if (address < imageSize) { |
YUPPY | 4:1354e56c7dd3 | 243 | char data[1024]; |
YUPPY | 4:1354e56c7dd3 | 244 | int size = readData(data, min(sizeof(data), imageSize - address), address); |
YUPPY | 4:1354e56c7dd3 | 245 | int ret = fwrite(data, size, 1, fp); |
YUPPY | 4:1354e56c7dd3 | 246 | if (ret > 0) |
YUPPY | 4:1354e56c7dd3 | 247 | address += size; |
YUPPY | 4:1354e56c7dd3 | 248 | if (ret == 0 || address >= imageSize) { |
YUPPY | 4:1354e56c7dd3 | 249 | stopPictures(); |
YUPPY | 4:1354e56c7dd3 | 250 | fclose(fp); |
YUPPY | 4:1354e56c7dd3 | 251 | wait(0.1); // ???? |
YUPPY | 4:1354e56c7dd3 | 252 | state = ret > 0 ? READY : ERROR; |
YUPPY | 4:1354e56c7dd3 | 253 | } |
YUPPY | 4:1354e56c7dd3 | 254 | } |
YUPPY | 4:1354e56c7dd3 | 255 | } |
YUPPY | 4:1354e56c7dd3 | 256 | |
YUPPY | 4:1354e56c7dd3 | 257 | return state == PROCESSING || state == READY; |
YUPPY | 3:5d0c4b13f4e8 | 258 | } |
YUPPY | 0:6212b283430c | 259 | |
YUPPY | 4:1354e56c7dd3 | 260 | bool JPEGCamera::reset() { |
YUPPY | 4:1354e56c7dd3 | 261 | char buf[4] = {0x56, 0x00, 0x26, 0x00}; |
YUPPY | 4:1354e56c7dd3 | 262 | int ret = sendReceive(buf, sizeof buf, 4); |
YUPPY | 4:1354e56c7dd3 | 263 | if (ret == 4 && buf[0] == 0x76) { |
YUPPY | 4:1354e56c7dd3 | 264 | wait(4.0); |
YUPPY | 4:1354e56c7dd3 | 265 | state = READY; |
YUPPY | 4:1354e56c7dd3 | 266 | } else { |
YUPPY | 4:1354e56c7dd3 | 267 | state = ERROR; |
YUPPY | 4:1354e56c7dd3 | 268 | } |
YUPPY | 4:1354e56c7dd3 | 269 | return state == READY; |
YUPPY | 4:1354e56c7dd3 | 270 | } |
YUPPY | 4:1354e56c7dd3 | 271 | |
YUPPY | 4:1354e56c7dd3 | 272 | bool JPEGCamera::takePicture() { |
YUPPY | 4:1354e56c7dd3 | 273 | char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00}; |
YUPPY | 4:1354e56c7dd3 | 274 | int ret = sendReceive(buf, sizeof buf, 5); |
YUPPY | 4:1354e56c7dd3 | 275 | |
YUPPY | 4:1354e56c7dd3 | 276 | return ret == 5 && buf[0] == 0x76; |
YUPPY | 4:1354e56c7dd3 | 277 | } |
YUPPY | 4:1354e56c7dd3 | 278 | |
YUPPY | 4:1354e56c7dd3 | 279 | bool JPEGCamera::stopPictures() { |
YUPPY | 4:1354e56c7dd3 | 280 | char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03}; |
YUPPY | 4:1354e56c7dd3 | 281 | int ret = sendReceive(buf, sizeof buf, 5); |
YUPPY | 4:1354e56c7dd3 | 282 | |
YUPPY | 4:1354e56c7dd3 | 283 | return ret == 4 && buf[0] == 0x76; |
YUPPY | 4:1354e56c7dd3 | 284 | } |
YUPPY | 4:1354e56c7dd3 | 285 | |
YUPPY | 4:1354e56c7dd3 | 286 | int JPEGCamera::getImageSize() { |
YUPPY | 4:1354e56c7dd3 | 287 | char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00}; |
YUPPY | 4:1354e56c7dd3 | 288 | int ret = sendReceive(buf, sizeof buf, 9); |
YUPPY | 4:1354e56c7dd3 | 289 | |
YUPPY | 4:1354e56c7dd3 | 290 | //The size is in the last 2 characters of the response. |
YUPPY | 4:1354e56c7dd3 | 291 | return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0; |
YUPPY | 4:1354e56c7dd3 | 292 | } |
YUPPY | 4:1354e56c7dd3 | 293 | |
YUPPY | 4:1354e56c7dd3 | 294 | int JPEGCamera::readData(char *dataBuf, int size, int address) { |
YUPPY | 4:1354e56c7dd3 | 295 | char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00, |
YUPPY | 4:1354e56c7dd3 | 296 | address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A |
YUPPY | 4:1354e56c7dd3 | 297 | }; |
YUPPY | 4:1354e56c7dd3 | 298 | int ret = sendReceive(buf, sizeof buf, 5); |
YUPPY | 4:1354e56c7dd3 | 299 | |
YUPPY | 4:1354e56c7dd3 | 300 | return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0; |
YUPPY | 4:1354e56c7dd3 | 301 | } |
YUPPY | 4:1354e56c7dd3 | 302 | |
YUPPY | 4:1354e56c7dd3 | 303 | int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) { |
YUPPY | 4:1354e56c7dd3 | 304 | while (readable()) getc(); |
YUPPY | 4:1354e56c7dd3 | 305 | |
YUPPY | 4:1354e56c7dd3 | 306 | for (int i = 0; i < sendSize; i++) putc(buf[i]); |
YUPPY | 4:1354e56c7dd3 | 307 | |
YUPPY | 4:1354e56c7dd3 | 308 | return receive(buf, receiveSize, RESPONSE_TIMEOUT); |
YUPPY | 4:1354e56c7dd3 | 309 | } |
YUPPY | 4:1354e56c7dd3 | 310 | |
YUPPY | 4:1354e56c7dd3 | 311 | int JPEGCamera::receive(char *buf, int size, int timeout) { |
YUPPY | 4:1354e56c7dd3 | 312 | timer.start(); |
YUPPY | 4:1354e56c7dd3 | 313 | timer.reset(); |
YUPPY | 4:1354e56c7dd3 | 314 | |
YUPPY | 4:1354e56c7dd3 | 315 | int i = 0; |
YUPPY | 4:1354e56c7dd3 | 316 | while (i < size && timer.read_ms() < timeout) { |
YUPPY | 4:1354e56c7dd3 | 317 | if (readable()) |
YUPPY | 4:1354e56c7dd3 | 318 | buf[i++] = getc(); |
YUPPY | 4:1354e56c7dd3 | 319 | } |
YUPPY | 4:1354e56c7dd3 | 320 | |
YUPPY | 4:1354e56c7dd3 | 321 | return i; |
YUPPY | 4:1354e56c7dd3 | 322 | } |
YUPPY | 5:5aa7223226df | 323 | void us015(){ |
YUPPY | 5:5aa7223226df | 324 | hs.TrigerOut(); |
YUPPY | 5:5aa7223226df | 325 | wait(1); |
YUPPY | 5:5aa7223226df | 326 | int distance; |
YUPPY | 5:5aa7223226df | 327 | distance = hs.GetDistance(); |
YUPPY | 5:5aa7223226df | 328 | printf("%d\r\n",distance);//距離出力 |
YUPPY | 5:5aa7223226df | 329 | } |
YUPPY | 4:1354e56c7dd3 | 330 | |
YUPPY | 5:5aa7223226df | 331 | void motorFoward(){ |
YUPPY | 5:5aa7223226df | 332 | left = 100; |
YUPPY | 5:5aa7223226df | 333 | right = 100; |
YUPPY | 5:5aa7223226df | 334 | printf("直進\n\r"); |
YUPPY | 5:5aa7223226df | 335 | } |
YUPPY | 5:5aa7223226df | 336 | void motorFowardR(){ |
YUPPY | 5:5aa7223226df | 337 | left = 100; |
YUPPY | 5:5aa7223226df | 338 | right = 20; |
YUPPY | 5:5aa7223226df | 339 | printf("右折\n\r"); |
YUPPY | 5:5aa7223226df | 340 | } |
YUPPY | 5:5aa7223226df | 341 | void motorForwardL(){ |
YUPPY | 5:5aa7223226df | 342 | left = 20; |
YUPPY | 5:5aa7223226df | 343 | right = 100; |
YUPPY | 5:5aa7223226df | 344 | printf("左折\n\r");} |
YUPPY | 5:5aa7223226df | 345 | void motorStop(){ |
YUPPY | 5:5aa7223226df | 346 | left = 0; |
YUPPY | 5:5aa7223226df | 347 | right = 0; |
YUPPY | 5:5aa7223226df | 348 | printf("停止\n\r");} |
YUPPY | 5:5aa7223226df | 349 | void motorReverse(){ |
YUPPY | 5:5aa7223226df | 350 | left = -50; |
YUPPY | 5:5aa7223226df | 351 | right = -50; |
YUPPY | 5:5aa7223226df | 352 | printf("バック\n\r"); |
YUPPY | 5:5aa7223226df | 353 | } |