岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

servo_and_movefunc.cpp@50:36741e8ab197, 2019-03-11 (annotated)

Committer:
shimizuta
Date:
Mon Mar 11 10:38:07 2019 +0000
Revision:
50:36741e8ab197
Parent:
43:2ed84f3558c1 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 43:2ed84f3558c1 1 #include "servo_and_movefunc.h"
shimizuta 43:2ed84f3558c1 2 #include "pi.h"
shimizuta 43:2ed84f3558c1 3 #include "math.h"
shimizuta 43:2ed84f3558c1 4 #include "debug.h"
shimizuta 43:2ed84f3558c1 5 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 6 #include "KondoServo.h"
shimizuta 43:2ed84f3558c1 7 #include "pinnames.h"
shimizuta 43:2ed84f3558c1 8 #include "can.h"
shimizuta 50:36741e8ab197 9 #include "hcsr04.h"
shimizuta 43:2ed84f3558c1 10 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
shimizuta 43:2ed84f3558c1 11 #endif
shimizuta 43:2ed84f3558c1 12 FILE *fp;
shimizuta 43:2ed84f3558c1 13 const float kRadToDegree = 180.0 / M_PI;
shimizuta 43:2ed84f3558c1 14 //サーボの送信間隔
shimizuta 43:2ed84f3558c1 15 const int kServoSpan_ms = 6;
shimizuta 43:2ed84f3558c1 16 //サーボの正負と座標系の正負の補正.足で一セット。
shimizuta 43:2ed84f3558c1 17 const int kServoSign[2][2] = {{-1, -1}, {-1, -1}};
shimizuta 43:2ed84f3558c1 18 //欲しい座標系0度でのサーボのICSマネージャーの値
shimizuta 43:2ed84f3558c1 19 const double kServoValToDegree = 270.0 / (11500 - 3500); //ICSの値を度に変換
shimizuta 43:2ed84f3558c1 20 const double kOriginDegree[2][2] = {
shimizuta 43:2ed84f3558c1 21 {
shimizuta 43:2ed84f3558c1 22 (7268 - 3500) * kServoValToDegree,
shimizuta 43:2ed84f3558c1 23 (7768 - 3500) * kServoValToDegree + 180,
shimizuta 43:2ed84f3558c1 24 },
shimizuta 43:2ed84f3558c1 25 {
shimizuta 43:2ed84f3558c1 26 (7467 - 3500) * kServoValToDegree,
shimizuta 43:2ed84f3558c1 27 (7543 - 3500) * kServoValToDegree + 180,
shimizuta 43:2ed84f3558c1 28 },
shimizuta 43:2ed84f3558c1 29 };
shimizuta 43:2ed84f3558c1 30 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 31 Timer timer;
shimizuta 43:2ed84f3558c1 32 KondoServo servo[2] = {
shimizuta 43:2ed84f3558c1 33 KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
shimizuta 43:2ed84f3558c1 34 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
shimizuta 43:2ed84f3558c1 35 };
shimizuta 43:2ed84f3558c1 36 DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
shimizuta 50:36741e8ab197 37 HCSR04 usensor(p25, p24); // Trigger(DO), Echo(PWMIN)
shimizuta 43:2ed84f3558c1 38 #endif
shimizuta 43:2ed84f3558c1 39 //一サイクル分進む.return 1:異常終了
shimizuta 43:2ed84f3558c1 40 int MoveOneCycle(Walk walkway, OneLeg leg[4])
shimizuta 43:2ed84f3558c1 41 {
shimizuta 43:2ed84f3558c1 42 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 43 timer.reset();
shimizuta 43:2ed84f3558c1 44 timer.start();
shimizuta 43:2ed84f3558c1 45 #endif
shimizuta 43:2ed84f3558c1 46 int count = walkway.orbit[0].GetOneWalkTime() / walkway.calctime_s_;
shimizuta 43:2ed84f3558c1 47 for (int i = 0; i < count; i++)
shimizuta 43:2ed84f3558c1 48 {
shimizuta 43:2ed84f3558c1 49 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 50 float time_s = timer.read();
shimizuta 43:2ed84f3558c1 51 #endif
shimizuta 43:2ed84f3558c1 52 //4本の足それぞれの足先サーボ角度更新
shimizuta 43:2ed84f3558c1 53 if (walkway.Cal4LegsPosi(leg) == 1)
shimizuta 43:2ed84f3558c1 54 {
shimizuta 43:2ed84f3558c1 55 printf("error: time = %f\r\n", i * walkway.calctime_s_);
shimizuta 43:2ed84f3558c1 56 return 1;
shimizuta 43:2ed84f3558c1 57 }
shimizuta 43:2ed84f3558c1 58 #ifdef USE_CAN
shimizuta 43:2ed84f3558c1 59 SendRad(leg[2], leg[3]); //slave_mbed分の足の目標位置を送信
shimizuta 43:2ed84f3558c1 60 #endif
shimizuta 43:2ed84f3558c1 61 //自身が動かす足のサーボを動かす
shimizuta 43:2ed84f3558c1 62 MoveServo(leg[0], 0, 0);
shimizuta 43:2ed84f3558c1 63 MoveServo(leg[1], 1, 0);
shimizuta 43:2ed84f3558c1 64 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 65 wait_ms(kServoSpan_ms);
shimizuta 43:2ed84f3558c1 66 #endif
shimizuta 43:2ed84f3558c1 67 MoveServo(leg[0], 0, 1);
shimizuta 43:2ed84f3558c1 68 MoveServo(leg[1], 1, 1);
shimizuta 43:2ed84f3558c1 69 #ifdef VSCODE
shimizuta 43:2ed84f3558c1 70 //ファイルに書き込み。time[s],x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[3]の順
shimizuta 43:2ed84f3558c1 71 fprintf(fp, "%f", i * walkway.calctime_s_);
shimizuta 43:2ed84f3558c1 72 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 73 fprintf(fp, ",%f,%f", leg[i].GetX_m(), leg[i].GetY_m());
shimizuta 43:2ed84f3558c1 74 fprintf(fp, "\r\n");
shimizuta 43:2ed84f3558c1 75 #else
shimizuta 43:2ed84f3558c1 76 //計算周期がwalkway.calctime_s_になるようwait
shimizuta 43:2ed84f3558c1 77 float rest_time_s = walkway.calctime_s_ - (timer.read() - time_s);
shimizuta 43:2ed84f3558c1 78 if (rest_time_s > 0)
shimizuta 43:2ed84f3558c1 79 wait(rest_time_s);
shimizuta 43:2ed84f3558c1 80 else
shimizuta 43:2ed84f3558c1 81 { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
shimizuta 43:2ed84f3558c1 82 DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 43:2ed84f3558c1 83 led[0] = 1;
shimizuta 43:2ed84f3558c1 84 }
shimizuta 43:2ed84f3558c1 85 #endif
shimizuta 43:2ed84f3558c1 86 }
shimizuta 43:2ed84f3558c1 87 return 0;
shimizuta 43:2ed84f3558c1 88 }
shimizuta 43:2ed84f3558c1 89 void MoveServo(OneLeg leg, int serial_num, int servo_id)
shimizuta 43:2ed84f3558c1 90 {
shimizuta 43:2ed84f3558c1 91 #ifndef VSCODE
shimizuta 43:2ed84f3558c1 92 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 43:2ed84f3558c1 93 //サーボの座標系に変更
shimizuta 43:2ed84f3558c1 94 float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
shimizuta 43:2ed84f3558c1 95 // DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree);
shimizuta 43:2ed84f3558c1 96 servo[serial_num].set_degree(servo_id, servo_degree);
shimizuta 43:2ed84f3558c1 97 #endif
shimizuta 43:2ed84f3558c1 98 }
shimizuta 43:2ed84f3558c1 99 void WaitStdin(char startchar)
shimizuta 43:2ed84f3558c1 100 {
shimizuta 43:2ed84f3558c1 101 char str[255] = {};
shimizuta 43:2ed84f3558c1 102 do
shimizuta 43:2ed84f3558c1 103 {
shimizuta 43:2ed84f3558c1 104 printf("put '%c', then start\r\n", startchar);
shimizuta 43:2ed84f3558c1 105 scanf("%s", str);
shimizuta 43:2ed84f3558c1 106 } while (str[0] != startchar);
shimizuta 43:2ed84f3558c1 107 }
shimizuta 43:2ed84f3558c1 108
shimizuta 43:2ed84f3558c1 109 int FileOpen() //1:異常終了
shimizuta 43:2ed84f3558c1 110 {
shimizuta 43:2ed84f3558c1 111 if ((fp = fopen("data.csv", "w")) == NULL)
shimizuta 43:2ed84f3558c1 112 {
shimizuta 43:2ed84f3558c1 113 printf("error : FileSave()\r\n");
shimizuta 43:2ed84f3558c1 114 return 1;
shimizuta 43:2ed84f3558c1 115 }
shimizuta 43:2ed84f3558c1 116 fprintf(fp, "time[s],x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[3]\r\n");
shimizuta 43:2ed84f3558c1 117 return 0;
shimizuta 43:2ed84f3558c1 118 }
shimizuta 50:36741e8ab197 119 unsigned int GetDist_cm()
shimizuta 50:36741e8ab197 120 {
shimizuta 50:36741e8ab197 121 usensor.start();
shimizuta 50:36741e8ab197 122 return usensor.get_dist_cm();
shimizuta 50:36741e8ab197 123 }