yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
servo/servo.cpp
- Committer:
- shimizuta
- Date:
- 2018-09-06
- Revision:
- 38:9bed85d32c97
- Parent:
- 37:d4b711619962
- Child:
- 39:ac2d5ae81fc1
File content as of revision 38:9bed85d32c97:
#include "servo.h" #include <mbed.h> #include "debug.h"//DEBUG("",変数);でデバッグ。 #include "pinnames.h" #include "coordinate.h" #include "KondoServo.h" #include "servo.h" const double kServoValToDegree = 270.0 / (11500 - 3500); //////////////////////////////変更できるパラメータ const int kServoSign[] = {-1, -1, -1, -1};//サーボの正負と座標系の正負の補正 const double kOriginDegree[SERVONUM - 1] = { (8850 - 3500) * kServoValToDegree , (8635 - 3500) * kServoValToDegree, (7533 - 3500) * kServoValToDegree, (8000 - 3500) * kServoValToDegree, };//初期状態の角度 const int kServoSpan_ms = 5; //指示の前後に必要なwait const double kOpenVal = (9000 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示 const double kCloseVal = (7000 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示 const int kTipServoID = 4; ////////////////////////////// KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:2,3,4 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:0, KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:1, }; double old_rad[SERVONUM] = {}; double OriginDegree[SERVONUM - 1] = {kOriginDegree[0], kOriginDegree[1], kOriginDegree[2], kOriginDegree[3]}; void ServoRad(int id, double rad_relative) { double degree = kServoSign[id] * rad_relative * kRadToDegree + OriginDegree[id]; switch(id) { case 0: servo[1].set_degree(id, degree); break; case 1: servo[2].set_degree(id, degree); break; case 2: case 3: case 4: servo[0].set_degree(id, degree); break; } SetNowRadRelative(id, old_rad[id]);//現在地変更はワンテンポ遅らせる(サーボの応答が遅いため) old_rad[id] = rad_relative; } //一般性は皆無 void ServoMoveOnArmInTurns() { static int count = 0; if(count == 0) { for(int i = 0; i < 3; i++) { double rad = GetNextRadRelative(i); ServoRad(i, rad); } } else if(count == 2) { double rad = GetNextRadRelative(3); ServoRad(3, rad); } ++count; if(count == 3) count = 0; } //一般性は皆無 void ServoMoveOnArm() { for(int i = 0; i < 4; i++) { ServoMoveOnArmInTurns(); wait_ms(kServoSpan_ms); } } void Open() { servo[0].set_degree(kTipServoID, kOpenVal); } void Close() { servo[0].set_degree(kTipServoID, kCloseVal); }