岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

servo_and_movefunc.cpp@43:2ed84f3558c1, 2019-03-06 (annotated)

Committer:
shimizuta
Date:
Wed Mar 06 12:13:46 2019 +0000
Revision:
43:2ed84f3558c1
Child:
50:36741e8ab197
moved 3/06

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