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?
User | Revision | Line number | New 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 | } |