test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp@8:21b932c4e6c5, 2019-02-10 (annotated)
- Committer:
- yuto17320508
- Date:
- Sun Feb 10 12:38:16 2019 +0000
- Revision:
- 8:21b932c4e6c5
- Parent:
- 7:72c80a7c20d6
- Child:
- 9:905f93247688
devide calculate process
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuto17320508 | 3:bcae0bb64b81 | 1 | //mbed間の同期を、master-slaveで行いたい。 |
yuto17320508 | 4:fffdb273836e | 2 | //masetrで4つの脚の動きをすべて決定, |
yuto17320508 | 4:fffdb273836e | 3 | //その動きをslaveに送る。 |
yuto17320508 | 3:bcae0bb64b81 | 4 | |
yuto17320508 | 0:f000d896d188 | 5 | #include "mbed.h" |
yuto17320508 | 0:f000d896d188 | 6 | #include "KondoServo.h" |
yuto17320508 | 6:43708adf2e5d | 7 | #define Mastar; |
yuto17320508 | 3:bcae0bb64b81 | 8 | //#define Slave; |
yuto17320508 | 7:72c80a7c20d6 | 9 | #define DebugMode; |
yuto17320508 | 4:fffdb273836e | 10 | |
yuto17320508 | 6:43708adf2e5d | 11 | //各値 |
yuto17320508 | 6:43708adf2e5d | 12 | namespace Parameters |
shimizuta | 2:a92568bdeb5c | 13 | { |
yuto17320508 | 7:72c80a7c20d6 | 14 | //パラメータ |
yuto17320508 | 7:72c80a7c20d6 | 15 | const float Pi = 3.14; |
yuto17320508 | 5:556d5a5e9d24 | 16 | const float kRadToDegree = 180.0 / Pi; |
yuto17320508 | 8:21b932c4e6c5 | 17 | const float tickerTime = 0.007f;//Ticker間隔 |
yuto17320508 | 6:43708adf2e5d | 18 | float OffsetY = 0.15f; |
yuto17320508 | 8:21b932c4e6c5 | 19 | float Stride_time_s = 0.8f;//床についている時間 1.0f |
yuto17320508 | 6:43708adf2e5d | 20 | float Stride_length_m = 0.05f;//歩幅 |
yuto17320508 | 8:21b932c4e6c5 | 21 | float Rising_time_s = 0.2f;//脚を上げている時間 0.2f |
yuto17320508 | 5:556d5a5e9d24 | 22 | float Width_m = 0.03f;//脚を上げる高さ |
yuto17320508 | 5:556d5a5e9d24 | 23 | static const int straight = 0; |
yuto17320508 | 5:556d5a5e9d24 | 24 | static const int turnleft = 1; |
yuto17320508 | 5:556d5a5e9d24 | 25 | static const int turnright = 2; |
yuto17320508 | 5:556d5a5e9d24 | 26 | static const int climb = 3; |
yuto17320508 | 5:556d5a5e9d24 | 27 | static const int overcoming = 4; |
yuto17320508 | 6:43708adf2e5d | 28 | } |
yuto17320508 | 6:43708adf2e5d | 29 | //定義 |
yuto17320508 | 6:43708adf2e5d | 30 | namespace Quadruped//4脚ロボット |
yuto17320508 | 6:43708adf2e5d | 31 | { |
yuto17320508 | 6:43708adf2e5d | 32 | using namespace Parameters; |
yuto17320508 | 7:72c80a7c20d6 | 33 | //USERが直接制御に使う関数 |
yuto17320508 | 6:43708adf2e5d | 34 | void SetOffsetY(float offsetY){OffsetY=offsetY;} |
yuto17320508 | 6:43708adf2e5d | 35 | void SetStrideMotion(float stride_time_s, float stride_length_m){Stride_time_s=stride_time_s;Stride_length_m=stride_length_m;}///////////////////////////////////設定 |
yuto17320508 | 6:43708adf2e5d | 36 | void SetRisiongMotion(float rising_time_s, float width_m){Rising_time_s=rising_time_s;Width_m =width_m;}////////////////////////////////////////設定 |
yuto17320508 | 6:43708adf2e5d | 37 | //ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。 |
yuto17320508 | 6:43708adf2e5d | 38 | int WalkMode = 0; |
yuto17320508 | 6:43708adf2e5d | 39 | void SetWalkMode(int mode){WalkMode = mode;} |
yuto17320508 | 5:556d5a5e9d24 | 40 | //4つの脚のtargetを保存する Legクラスで後で使えるようにしておく |
yuto17320508 | 6:43708adf2e5d | 41 | float TargetX[4]; |
yuto17320508 | 6:43708adf2e5d | 42 | float TargetY[4]; |
yuto17320508 | 5:556d5a5e9d24 | 43 | //脚の定義 位置を入力するとその方向にサーボが動く |
yuto17320508 | 5:556d5a5e9d24 | 44 | class Leg; |
yuto17320508 | 5:556d5a5e9d24 | 45 | //脚ごとのモーション これを組み合わせる |
yuto17320508 | 5:556d5a5e9d24 | 46 | namespace Motion{} |
yuto17320508 | 5:556d5a5e9d24 | 47 | //モーションを組み合わせて歩行パターンを形成する |
yuto17320508 | 5:556d5a5e9d24 | 48 | //前進、旋回など |
yuto17320508 | 5:556d5a5e9d24 | 49 | namespace WalkingPattern{} |
yuto17320508 | 5:556d5a5e9d24 | 50 | //パターン関数を呼び出す 一つの関数でいいようにswitchする |
yuto17320508 | 5:556d5a5e9d24 | 51 | void CalcWalk(int legNum, Leg leg); |
yuto17320508 | 5:556d5a5e9d24 | 52 | } // namespace Quadruped |
yuto17320508 | 4:fffdb273836e | 53 | |
yuto17320508 | 4:fffdb273836e | 54 | namespace CanConnector |
yuto17320508 | 4:fffdb273836e | 55 | { |
yuto17320508 | 6:43708adf2e5d | 56 | //Maserとslaveの定義 同じ関数名で動くようにしたい |
yuto17320508 | 7:72c80a7c20d6 | 57 | class MastarConnector;//cansendするマイコンの場合 |
yuto17320508 | 7:72c80a7c20d6 | 58 | class SlaveConnector;//canreadするマイコンの場合 |
yuto17320508 | 4:fffdb273836e | 59 | } |
yuto17320508 | 4:fffdb273836e | 60 | |
yuto17320508 | 4:fffdb273836e | 61 | //実装 |
yuto17320508 | 4:fffdb273836e | 62 | namespace Quadruped |
yuto17320508 | 4:fffdb273836e | 63 | { |
yuto17320508 | 5:556d5a5e9d24 | 64 | //脚の定義 |
yuto17320508 | 5:556d5a5e9d24 | 65 | class Leg |
yuto17320508 | 5:556d5a5e9d24 | 66 | { |
yuto17320508 | 5:556d5a5e9d24 | 67 | float rad_[2]; |
yuto17320508 | 5:556d5a5e9d24 | 68 | int id_[2]; |
yuto17320508 | 5:556d5a5e9d24 | 69 | //パラメータ。実際の機体に合わせていじる |
yuto17320508 | 5:556d5a5e9d24 | 70 | static const float dist_between_servo_half_m_ = 0.06f * 0.5; |
yuto17320508 | 5:556d5a5e9d24 | 71 | static const float LegLength1 = 0.1f; |
yuto17320508 | 5:556d5a5e9d24 | 72 | static const float LegLength2 = 0.2f; |
yuto17320508 | 5:556d5a5e9d24 | 73 | KondoServo servo_; |
yuto17320508 | 3:bcae0bb64b81 | 74 | |
yuto17320508 | 5:556d5a5e9d24 | 75 | public: |
yuto17320508 | 5:556d5a5e9d24 | 76 | Leg(PinName pin_serial_tx, PinName pin_serial_rx); |
yuto17320508 | 5:556d5a5e9d24 | 77 | void MoveServo(int servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 78 | void CalServoRad(float x_m, float y_m); |
yuto17320508 | 5:556d5a5e9d24 | 79 | void SetRad(float rad, int servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 80 | float GetRad(int servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 81 | }; |
yuto17320508 | 5:556d5a5e9d24 | 82 | |
yuto17320508 | 5:556d5a5e9d24 | 83 | namespace Motion |
yuto17320508 | 5:556d5a5e9d24 | 84 | { |
yuto17320508 | 7:72c80a7c20d6 | 85 | void Stride(float time_s,int legnum); |
yuto17320508 | 7:72c80a7c20d6 | 86 | void Rising(float time_s,int legnum); |
yuto17320508 | 7:72c80a7c20d6 | 87 | void Stop(int legnum); |
yuto17320508 | 5:556d5a5e9d24 | 88 | } // namespace Motion |
yuto17320508 | 5:556d5a5e9d24 | 89 | |
yuto17320508 | 5:556d5a5e9d24 | 90 | namespace WalkingPattern |
yuto17320508 | 5:556d5a5e9d24 | 91 | { |
yuto17320508 | 5:556d5a5e9d24 | 92 | using namespace Motion; |
yuto17320508 | 8:21b932c4e6c5 | 93 | void Straight(int servo_num); |
yuto17320508 | 8:21b932c4e6c5 | 94 | void TurnLeft(int servo_num); |
yuto17320508 | 8:21b932c4e6c5 | 95 | void TurnRight(int servo_num); |
yuto17320508 | 8:21b932c4e6c5 | 96 | void Climb(int servo_num); |
yuto17320508 | 8:21b932c4e6c5 | 97 | void Overcoming(int servo_num); //段差、紐乗り越え動作 |
yuto17320508 | 5:556d5a5e9d24 | 98 | } // namespace WalkingPattern |
yuto17320508 | 3:bcae0bb64b81 | 99 | |
yuto17320508 | 5:556d5a5e9d24 | 100 | Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) |
yuto17320508 | 5:556d5a5e9d24 | 101 | { |
yuto17320508 | 5:556d5a5e9d24 | 102 | rad_[0] = 0; |
yuto17320508 | 5:556d5a5e9d24 | 103 | rad_[1] = Pi; |
yuto17320508 | 5:556d5a5e9d24 | 104 | }; |
yuto17320508 | 5:556d5a5e9d24 | 105 | void Leg::MoveServo(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 106 | { |
yuto17320508 | 5:556d5a5e9d24 | 107 | float degree = GetRad(servo_num) * kRadToDegree; |
yuto17320508 | 5:556d5a5e9d24 | 108 | //servo1は反転させる |
yuto17320508 | 5:556d5a5e9d24 | 109 | if (servo_num == 0) |
yuto17320508 | 5:556d5a5e9d24 | 110 | degree += 90; |
yuto17320508 | 5:556d5a5e9d24 | 111 | else |
yuto17320508 | 5:556d5a5e9d24 | 112 | degree = 270 - degree; |
yuto17320508 | 5:556d5a5e9d24 | 113 | servo_.set_degree(servo_num, degree); |
yuto17320508 | 5:556d5a5e9d24 | 114 | } |
yuto17320508 | 5:556d5a5e9d24 | 115 | void Leg::CalServoRad(float x_m, float y_m) |
yuto17320508 | 5:556d5a5e9d24 | 116 | { |
yuto17320508 | 5:556d5a5e9d24 | 117 | //処理を軽くするために共通部分は先に計算 |
yuto17320508 | 5:556d5a5e9d24 | 118 | float temp_x[] = {x_m + dist_between_servo_half_m_, |
yuto17320508 | 5:556d5a5e9d24 | 119 | x_m - dist_between_servo_half_m_}; |
yuto17320508 | 5:556d5a5e9d24 | 120 | float temp_y2 = y_m * y_m; |
yuto17320508 | 5:556d5a5e9d24 | 121 | float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2; |
yuto17320508 | 4:fffdb273836e | 122 | |
yuto17320508 | 5:556d5a5e9d24 | 123 | float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); |
yuto17320508 | 5:556d5a5e9d24 | 124 | float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); |
yuto17320508 | 5:556d5a5e9d24 | 125 | float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)), |
yuto17320508 | 5:556d5a5e9d24 | 126 | atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))}; |
yuto17320508 | 5:556d5a5e9d24 | 127 | for (size_t i = 0; i < 2; i++) |
yuto17320508 | 5:556d5a5e9d24 | 128 | SetRad(targetTheta[i], i); |
yuto17320508 | 5:556d5a5e9d24 | 129 | } |
yuto17320508 | 5:556d5a5e9d24 | 130 | void Leg::SetRad(float rad, int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 131 | { |
yuto17320508 | 5:556d5a5e9d24 | 132 | rad_[servo_num] = rad; |
yuto17320508 | 5:556d5a5e9d24 | 133 | } |
yuto17320508 | 5:556d5a5e9d24 | 134 | float Leg::GetRad(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 135 | { |
yuto17320508 | 5:556d5a5e9d24 | 136 | return rad_[servo_num]; |
shimizuta | 2:a92568bdeb5c | 137 | } |
shimizuta | 2:a92568bdeb5c | 138 | |
yuto17320508 | 3:bcae0bb64b81 | 139 | |
yuto17320508 | 7:72c80a7c20d6 | 140 | void Motion::Stride(float time_s,int legnum) |
yuto17320508 | 5:556d5a5e9d24 | 141 | { |
yuto17320508 | 7:72c80a7c20d6 | 142 | TargetX[legnum] = (Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f); |
yuto17320508 | 7:72c80a7c20d6 | 143 | TargetY[legnum] = 0.0f; |
yuto17320508 | 7:72c80a7c20d6 | 144 | |
yuto17320508 | 5:556d5a5e9d24 | 145 | } |
yuto17320508 | 5:556d5a5e9d24 | 146 | |
yuto17320508 | 7:72c80a7c20d6 | 147 | void Motion::Rising(float time_s,int legnum) |
yuto17320508 | 5:556d5a5e9d24 | 148 | { |
yuto17320508 | 7:72c80a7c20d6 | 149 | TargetX[legnum] = (Stride_length_m/2.0f)*cos(-Pi*time_s/Rising_time_s); |
yuto17320508 | 7:72c80a7c20d6 | 150 | TargetY[legnum] = Width_m*sin(-Pi*time_s/Rising_time_s); |
yuto17320508 | 7:72c80a7c20d6 | 151 | |
yuto17320508 | 5:556d5a5e9d24 | 152 | } |
yuto17320508 | 5:556d5a5e9d24 | 153 | |
yuto17320508 | 7:72c80a7c20d6 | 154 | float walkPeriod = Stride_time_s+Rising_time_s;//周期 |
yuto17320508 | 7:72c80a7c20d6 | 155 | float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差 |
yuto17320508 | 7:72c80a7c20d6 | 156 | |
yuto17320508 | 7:72c80a7c20d6 | 157 | |
yuto17320508 | 8:21b932c4e6c5 | 158 | void WalkingPattern::Straight(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 159 | { |
yuto17320508 | 8:21b932c4e6c5 | 160 | int servo_state = 2*servo_num; |
yuto17320508 | 6:43708adf2e5d | 161 | static float time_s = 0.0f;//共通時間 |
yuto17320508 | 8:21b932c4e6c5 | 162 | for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する |
yuto17320508 | 5:556d5a5e9d24 | 163 | { |
yuto17320508 | 6:43708adf2e5d | 164 | float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間 |
yuto17320508 | 7:72c80a7c20d6 | 165 | if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i); |
yuto17320508 | 7:72c80a7c20d6 | 166 | else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i); |
yuto17320508 | 7:72c80a7c20d6 | 167 | else Motion::Stride(thisLegTime-walkPeriod,i); |
yuto17320508 | 5:556d5a5e9d24 | 168 | } |
yuto17320508 | 7:72c80a7c20d6 | 169 | #ifdef DebugMode |
yuto17320508 | 8:21b932c4e6c5 | 170 | if(servo_num == 0) |
yuto17320508 | 8:21b932c4e6c5 | 171 | printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0], TargetY[0]); |
yuto17320508 | 7:72c80a7c20d6 | 172 | #endif |
yuto17320508 | 8:21b932c4e6c5 | 173 | time_s += tickerTime; |
yuto17320508 | 8:21b932c4e6c5 | 174 | if(time_s >= walkPeriod) |
yuto17320508 | 8:21b932c4e6c5 | 175 | { |
yuto17320508 | 8:21b932c4e6c5 | 176 | time_s = 0.0f; |
yuto17320508 | 8:21b932c4e6c5 | 177 | } |
yuto17320508 | 5:556d5a5e9d24 | 178 | } |
yuto17320508 | 8:21b932c4e6c5 | 179 | void WalkingPattern::TurnLeft(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 180 | { |
yuto17320508 | 6:43708adf2e5d | 181 | //static float time_s = 0.0f; |
yuto17320508 | 5:556d5a5e9d24 | 182 | |
yuto17320508 | 5:556d5a5e9d24 | 183 | } |
yuto17320508 | 8:21b932c4e6c5 | 184 | void WalkingPattern::TurnRight(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 185 | { |
yuto17320508 | 6:43708adf2e5d | 186 | //static float time_s = 0.0f; |
yuto17320508 | 5:556d5a5e9d24 | 187 | |
yuto17320508 | 5:556d5a5e9d24 | 188 | } |
yuto17320508 | 8:21b932c4e6c5 | 189 | void WalkingPattern::Climb(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 190 | { |
yuto17320508 | 6:43708adf2e5d | 191 | //static float time_s = 0.0f; |
yuto17320508 | 5:556d5a5e9d24 | 192 | |
yuto17320508 | 5:556d5a5e9d24 | 193 | } |
yuto17320508 | 8:21b932c4e6c5 | 194 | void WalkingPattern::Overcoming(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 195 | { |
yuto17320508 | 6:43708adf2e5d | 196 | //static float time_s = 0.0f; |
yuto17320508 | 5:556d5a5e9d24 | 197 | |
yuto17320508 | 5:556d5a5e9d24 | 198 | } |
yuto17320508 | 5:556d5a5e9d24 | 199 | |
yuto17320508 | 8:21b932c4e6c5 | 200 | void CalcWalk(int servo_num) |
yuto17320508 | 5:556d5a5e9d24 | 201 | { |
yuto17320508 | 5:556d5a5e9d24 | 202 | switch(WalkMode) |
yuto17320508 | 5:556d5a5e9d24 | 203 | { |
yuto17320508 | 5:556d5a5e9d24 | 204 | case straight: |
yuto17320508 | 8:21b932c4e6c5 | 205 | WalkingPattern::Straight(servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 206 | break; |
yuto17320508 | 5:556d5a5e9d24 | 207 | case turnleft: |
yuto17320508 | 8:21b932c4e6c5 | 208 | WalkingPattern::TurnLeft(servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 209 | break; |
yuto17320508 | 5:556d5a5e9d24 | 210 | case turnright: |
yuto17320508 | 8:21b932c4e6c5 | 211 | WalkingPattern::TurnRight(servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 212 | break; |
yuto17320508 | 5:556d5a5e9d24 | 213 | case climb: |
yuto17320508 | 8:21b932c4e6c5 | 214 | WalkingPattern::Climb(servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 215 | break; |
yuto17320508 | 5:556d5a5e9d24 | 216 | case overcoming: |
yuto17320508 | 8:21b932c4e6c5 | 217 | WalkingPattern::Overcoming(servo_num); |
yuto17320508 | 5:556d5a5e9d24 | 218 | break; |
yuto17320508 | 5:556d5a5e9d24 | 219 | } |
yuto17320508 | 5:556d5a5e9d24 | 220 | } |
yuto17320508 | 5:556d5a5e9d24 | 221 | } // namespace Quadruped |
shimizuta | 2:a92568bdeb5c | 222 | |
yuto17320508 | 6:43708adf2e5d | 223 | namespace CanConnector |
yuto17320508 | 6:43708adf2e5d | 224 | { |
yuto17320508 | 6:43708adf2e5d | 225 | class MastarConnector |
yuto17320508 | 6:43708adf2e5d | 226 | { |
yuto17320508 | 6:43708adf2e5d | 227 | public: |
yuto17320508 | 6:43708adf2e5d | 228 | MastarConnector(PinName rd, PinName td); |
yuto17320508 | 6:43708adf2e5d | 229 | CAN can_; |
yuto17320508 | 7:72c80a7c20d6 | 230 | void Connect(); |
yuto17320508 | 6:43708adf2e5d | 231 | |
yuto17320508 | 6:43708adf2e5d | 232 | }; |
yuto17320508 | 7:72c80a7c20d6 | 233 | MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){} |
yuto17320508 | 6:43708adf2e5d | 234 | |
yuto17320508 | 6:43708adf2e5d | 235 | |
yuto17320508 | 6:43708adf2e5d | 236 | class SlaveConnector |
yuto17320508 | 6:43708adf2e5d | 237 | { |
yuto17320508 | 6:43708adf2e5d | 238 | public: |
yuto17320508 | 6:43708adf2e5d | 239 | SlaveConnector(PinName rd, PinName td); |
yuto17320508 | 6:43708adf2e5d | 240 | CAN can_; |
yuto17320508 | 7:72c80a7c20d6 | 241 | void Connect(); |
yuto17320508 | 6:43708adf2e5d | 242 | |
yuto17320508 | 6:43708adf2e5d | 243 | }; |
yuto17320508 | 7:72c80a7c20d6 | 244 | SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){} |
yuto17320508 | 6:43708adf2e5d | 245 | |
yuto17320508 | 6:43708adf2e5d | 246 | } |
yuto17320508 | 6:43708adf2e5d | 247 | |
yuto17320508 | 3:bcae0bb64b81 | 248 | //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 |
yuto17320508 | 3:bcae0bb64b81 | 249 | using namespace Quadruped; |
yuto17320508 | 3:bcae0bb64b81 | 250 | Leg leg1(p9, p10); |
yuto17320508 | 5:556d5a5e9d24 | 251 | Leg leg2(p28, p27); |
yuto17320508 | 7:72c80a7c20d6 | 252 | //CAN can1(p30, p29); |
shimizuta | 2:a92568bdeb5c | 253 | Ticker fliper; |
yuto17320508 | 7:72c80a7c20d6 | 254 | #ifdef Mastar |
yuto17320508 | 7:72c80a7c20d6 | 255 | CanConnector::MastarConnector connector(p30,p29); |
yuto17320508 | 7:72c80a7c20d6 | 256 | #elif Slave |
yuto17320508 | 7:72c80a7c20d6 | 257 | CanConnector::SlaveConnector connector(p30,p29); |
yuto17320508 | 7:72c80a7c20d6 | 258 | #endif |
yuto17320508 | 5:556d5a5e9d24 | 259 | |
yuto17320508 | 0:f000d896d188 | 260 | void cb_timer() |
yuto17320508 | 0:f000d896d188 | 261 | { |
shimizuta | 2:a92568bdeb5c | 262 | static int servo_num = 0; |
shimizuta | 2:a92568bdeb5c | 263 | //出力。片サーボずつ |
yuto17320508 | 3:bcae0bb64b81 | 264 | leg1.MoveServo(servo_num); |
yuto17320508 | 3:bcae0bb64b81 | 265 | leg2.MoveServo(servo_num); |
yuto17320508 | 8:21b932c4e6c5 | 266 | Quadruped::CalcWalk(servo_num); |
shimizuta | 2:a92568bdeb5c | 267 | ++servo_num; |
yuto17320508 | 8:21b932c4e6c5 | 268 | if(servo_num > 1)servo_num = 0; |
yuto17320508 | 5:556d5a5e9d24 | 269 | |
yuto17320508 | 8:21b932c4e6c5 | 270 | /*if (servo_num > 1) //両サーボ出力したら |
yuto17320508 | 0:f000d896d188 | 271 | { |
shimizuta | 2:a92568bdeb5c | 272 | //目標角の更新 |
yuto17320508 | 8:21b932c4e6c5 | 273 | Quadruped::CalcWalk(servo_num); |
yuto17320508 | 6:43708adf2e5d | 274 | //このマイコンで使う脚だけ計算 |
yuto17320508 | 6:43708adf2e5d | 275 | leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); |
yuto17320508 | 6:43708adf2e5d | 276 | leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); |
yuto17320508 | 7:72c80a7c20d6 | 277 | |
shimizuta | 2:a92568bdeb5c | 278 | servo_num = 0; |
yuto17320508 | 8:21b932c4e6c5 | 279 | }*/ |
yuto17320508 | 5:556d5a5e9d24 | 280 | |
yuto17320508 | 0:f000d896d188 | 281 | } |
shimizuta | 2:a92568bdeb5c | 282 | int main() |
yuto17320508 | 0:f000d896d188 | 283 | { |
yuto17320508 | 8:21b932c4e6c5 | 284 | Quadruped::CalcWalk(0); |
yuto17320508 | 8:21b932c4e6c5 | 285 | Quadruped::CalcWalk(1); |
yuto17320508 | 6:43708adf2e5d | 286 | leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); |
yuto17320508 | 6:43708adf2e5d | 287 | leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); |
yuto17320508 | 0:f000d896d188 | 288 | fliper.attach(&cb_timer, tickerTime); |
yuto17320508 | 0:f000d896d188 | 289 | } |