CansatB_2021
６足歩行試験用プログラム 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
--- /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
--- /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
--- /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
--- /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センテンスの情報が欠けている時
+}
--- /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
--- /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
--- /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