CansatB_2021
/
gilatina_6legs_test
6足歩行試験用プログラム Movement のdefine の値を変えると方向変換
Revision 0:6b57aea7ad9b, committed 2021-12-06
- Comitter:
- minanao
- Date:
- Mon Dec 06 08:54:49 2021 +0000
- Commit message:
- 6legs_test
Changed in this revision
diff -r 000000000000 -r 6b57aea7ad9b Movement.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Movement.cpp Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,93 @@ +#include "mbed.h" +#include "Servo.h" +#include "Movement.h" + +//上から順にservo1.2.3.4.5.6の基準値 これを変えればすべての値が変わる +#define S1 1.0 +#define S2 1.0 +#define S3 1.0 +#define S4 1.0 +#define S5 1.0 +#define S6 1.0 + + +//前と後ろ +Servo servo1(D7); +Servo servo3(A1); +Servo servo4(D12); +Servo servo6(A5); + +//新しく取り付けるもの +Servo servo2(A3); +Servo servo5(D10); + + void Movement::stop(){ + servo1 = S1/2; + servo3 = S3/2; + servo4 = S4/2; + servo6 = S6/2; + + // + servo2 = S2/2; + servo5 = S5/2; + wait(1); + } + +//前進 + void Movement::move_forward(int time = 20) + { + servo1 = 0; + servo2 = 0; + servo3 = 0; + servo4 = 0; + servo5 = 0; + servo6 = 0; + wait(time); + } + +//後退 + void Movement::move_backward() + { + servo1 = S1; + servo2 = S2; + servo3 = S3; + servo4 = S4; + servo5 = S5; + servo6 = S6; + wait(5); + } + +//右に曲がる + void Movement::turn_right(int theta = 15) + { + servo1 = S1; + servo2 = S2; + servo3 = S3; + servo4 = 0; + servo5 = 0; + servo6 = 0; + wait(int(theta/15)); + } + +//左に曲がる + void Movement::turn_left(int theta = 15) + { + servo1 = 0; + servo2 = 0; + servo3 = 0; + servo4 = S4; + servo5 = S5; + servo6 = S6; + wait(int(theta/15)); + } + +//倒れているときの処理 +void Movement::wakeup(int time){ + for(int i=1;i<=time;i++) + { + move_forward(5); + move_backward(); + turn_right(); + turn_left(); + } +} \ No newline at end of file
diff -r 000000000000 -r 6b57aea7ad9b Movement.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Movement.h Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,25 @@ +#ifndef MOVEMENT_H +#define MOVEMENT_H + +#include "mbed.h" +#include "Servo.h" + +class Movement +{ +public: + +//停止 + void stop(); +//前進 + void move_forward(int time); +//後退 + void move_backward(); +//右に曲がる + void turn_right(int theta); +//左に曲がる + void turn_left(int theta); +//横転から復帰 + void wakeup(int time); +}; + +#endif \ No newline at end of file
diff -r 000000000000 -r 6b57aea7ad9b Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 6b57aea7ad9b getGPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.cpp Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,57 @@ +#include "mbed.h" +#include "getGPS.h" + +GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) +{ + latitude = 0; + longitude = 0; + _gps.baud(GPSBAUD); + _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); +} + +bool GPS::getgps() +{ + char gps_data[256]; + int i; + + do { + while(_gps.getc() != '$'); //$マークまで読み飛ばし + i = 0; + + /* gpa_data初期化 */ + for(int j = 0; j < 256; j++) + gps_data[j] = '\0'; + + /* NMEAから一行読み込み */ + while((gps_data[i] = _gps.getc()) != '\r') { + i++; + if(i == 256) { + i = 255; + break; + } + } + } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける + + int rlock; + char ns,ew; + double w_time, raw_longitude, raw_latitude; + int satnum; + double hdop; + + if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) { + /* 座標1(度部分) */ + int latitude_dd = (int)(raw_latitude / 100); + int longitude_dd = (int)(raw_longitude / 100); + + /* 座標2(分部分 → 度) */ + double latitude_md = (raw_latitude - latitude_dd * 100) / 60; + double longitude_md = (raw_longitude - longitude_dd * 100) / 60; + + /* 座標1 + 2 */ + latitude = latitude_dd + latitude_md; + longitude = longitude_dd + longitude_md; + + return true; + } else + return false; //GGAセンテンスの情報が欠けている時 +}
diff -r 000000000000 -r 6b57aea7ad9b getGPS.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.h Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,19 @@ +#ifndef GPS_H +#define GPS_H + +#define GPSBAUD 9600 //ボーレート + +class GPS { +public: + GPS(PinName gpstx,PinName gpsrx); + + bool getgps(); + + double longitude; + double latitude; + +private: + Serial _gps; +}; + +#endif //GPS_H \ No newline at end of file
diff -r 000000000000 -r 6b57aea7ad9b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,16 @@ +#include "mbed.h" +#include "Movement.h" + +int main(){ + Movement idou; + //5秒停止 + idou.stop(); + wait(3); + idou.move_backward(); + wait(3); + idou.turn_right(360); + idou.turn_left(360); + idou.stop(); + + return 0; +} \ No newline at end of file
diff -r 000000000000 -r 6b57aea7ad9b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 06 08:54:49 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file