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?
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 | 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 | } |