yuto kawamura / Mbed 2 deprecated cat18_operate_last

yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

servo/servo.cpp@37:d4b711619962, 2018-09-05 (annotated)

Committer:
shimizuta
Date:
Wed Sep 05 09:37:41 2018 +0000
Revision:
37:d4b711619962
Parent:
36:b8171eeefcd0
Child:
38:9bed85d32c97
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 3:05b1dcb3634e 1 #include "servo.h"
shimizuta 0:c3a72fe24137 2 #include <mbed.h>
shimizuta 0:c3a72fe24137 3 #include "debug.h"//DEBUG("",変数);でデバッグ。
shimizuta 0:c3a72fe24137 4 #include "pinnames.h"
shimizuta 4:187c62291654 5 #include "coordinate.h"
shimizuta 2:4515e8cc6b60 6 #include "KondoServo.h"
shimizuta 37:d4b711619962 7 #include "servo.h"
shimizuta 23:f45eb02433a5 8 const double kServoValToDegree = 270.0 / (11500 - 3500);
shimizuta 19:48c3af917932 9 //////////////////////////////変更できるパラメータ
shimizuta 24:b4a89db4ed72 10 const int kServoSign[] = {-1, -1, -1, -1};//サーボの正負と座標系の正負の補正
shimizuta 37:d4b711619962 11 const double kOriginDegree[SERVONUM - 1] = {
shimizuta 36:b8171eeefcd0 12 (8850 - 3500) * kServoValToDegree ,
shimizuta 30:af136b83b202 13 (8635 - 3500) * kServoValToDegree,
shimizuta 30:af136b83b202 14 (7533 - 3500) * kServoValToDegree,
shimizuta 24:b4a89db4ed72 15 (8000 - 3500) * kServoValToDegree,
shimizuta 12:174f0090aa79 16 };//初期状態の角度
shimizuta 37:d4b711619962 17
shimizuta 37:d4b711619962 18 const int kServoSpan_ms = 5; //指示の前後に必要なwait
shimizuta 37:d4b711619962 19 const double kOpenVal = (9000 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
shimizuta 37:d4b711619962 20 const double kCloseVal = (7000 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示
shimizuta 20:13934809e117 21 const int kTipServoID = 4;
shimizuta 19:48c3af917932 22 //////////////////////////////
shimizuta 21:af9a33d69745 23 KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:2,3,4
shimizuta 21:af9a33d69745 24 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:0,
shimizuta 21:af9a33d69745 25 KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:1,
shimizuta 20:13934809e117 26 };
shimizuta 23:f45eb02433a5 27 double old_rad[SERVONUM] = {};
shimizuta 37:d4b711619962 28 double OriginDegree[SERVONUM - 1] = {kOriginDegree[0], kOriginDegree[1], kOriginDegree[2], kOriginDegree[3]};
shimizuta 20:13934809e117 29 void ServoRad(int id, double rad_relative)
shimizuta 19:48c3af917932 30 {
shimizuta 37:d4b711619962 31 double degree = kServoSign[id] * rad_relative * kRadToDegree + OriginDegree[id];
shimizuta 20:13934809e117 32 switch(id) {
shimizuta 20:13934809e117 33 case 0:
shimizuta 20:13934809e117 34 servo[1].set_degree(id, degree);
shimizuta 20:13934809e117 35 break;
shimizuta 20:13934809e117 36 case 1:
shimizuta 20:13934809e117 37 servo[2].set_degree(id, degree);
shimizuta 20:13934809e117 38 break;
shimizuta 20:13934809e117 39 case 2:
shimizuta 20:13934809e117 40 case 3:
shimizuta 20:13934809e117 41 case 4:
shimizuta 20:13934809e117 42 servo[0].set_degree(id, degree);
shimizuta 20:13934809e117 43 break;
shimizuta 20:13934809e117 44 }
shimizuta 23:f45eb02433a5 45 SetNowRadRelative(id, old_rad[id]);//現在地変更はワンテンポ遅らせる(サーボの応答が遅いため)
shimizuta 23:f45eb02433a5 46 old_rad[id] = rad_relative;
shimizuta 13:126c3f7f9b89 47 }
shimizuta 21:af9a33d69745 48 //一般性は皆無
shimizuta 19:48c3af917932 49 void ServoMoveOnArmInTurns()
shimizuta 19:48c3af917932 50 {
shimizuta 20:13934809e117 51 static int count = 0;
shimizuta 21:af9a33d69745 52 if(count == 0) {
shimizuta 21:af9a33d69745 53 for(int i = 0; i < 3; i++) {
shimizuta 21:af9a33d69745 54 double rad = GetNextRadRelative(i);
shimizuta 21:af9a33d69745 55 ServoRad(i, rad);
shimizuta 21:af9a33d69745 56 }
shimizuta 23:f45eb02433a5 57 } else if(count == 2) {
shimizuta 21:af9a33d69745 58 double rad = GetNextRadRelative(3);
shimizuta 21:af9a33d69745 59 ServoRad(3, rad);
shimizuta 21:af9a33d69745 60 }
shimizuta 20:13934809e117 61 ++count;
shimizuta 23:f45eb02433a5 62 if(count == 3) count = 0;
shimizuta 19:48c3af917932 63 }
shimizuta 21:af9a33d69745 64 //一般性は皆無
shimizuta 4:187c62291654 65 void ServoMoveOnArm()
shimizuta 3:05b1dcb3634e 66 {
shimizuta 30:af136b83b202 67 for(int i = 0; i < 4; i++) {
shimizuta 19:48c3af917932 68 ServoMoveOnArmInTurns();
shimizuta 3:05b1dcb3634e 69 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 70 }
shimizuta 3:05b1dcb3634e 71 }
shimizuta 3:05b1dcb3634e 72 void Open()
shimizuta 3:05b1dcb3634e 73 {
shimizuta 24:b4a89db4ed72 74 servo[0].set_degree(kTipServoID, kOpenVal);
shimizuta 25:55f7409c3502 75 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 76 }
shimizuta 2:4515e8cc6b60 77 void Close()
shimizuta 2:4515e8cc6b60 78 {
yuto17320508 33:3902febe8c45 79 for(int i = 0; i < 5; i++) {
yuto17320508 32:21229c96144a 80 servo[0].set_degree(kTipServoID, kCloseVal);
yuto17320508 32:21229c96144a 81 wait_ms(kServoSpan_ms);
yuto17320508 32:21229c96144a 82 }
shimizuta 5:af5ccfce1b90 83 }