yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
Diff: servo/servo.cpp
- Revision:
- 13:126c3f7f9b89
- Parent:
- 12:174f0090aa79
- Child:
- 16:b2358fd35999
--- a/servo/servo.cpp Mon Aug 13 02:22:58 2018 +0000 +++ b/servo/servo.cpp Mon Aug 13 09:03:30 2018 +0000 @@ -15,16 +15,18 @@ const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正 -const int kServoSpan_ms = 5; //指示の前後に必要なwait +const int kServoSpan_ms = 2; //指示の前後に必要なwait void Open(); void Move(double x, double y, double theta); - +void ServoRad(int i, double rad){ + servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]); +} void ServoMoveOnArm() { for(int i = 0; i < kServoNum; i++) { double rad = GetNextRadRelative(i); - servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]); + ServoRad(i, rad); SetNowRadRelative(i,rad); wait_ms(kServoSpan_ms); }