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@29:ea48140fc85e, 2018-09-04 (annotated)

Committer:
shimizuta
Date:
Tue Sep 04 12:53:50 2018 +0000
Revision:
29:ea48140fc85e
Parent:
28:bcfc84b481bc
Child:
30:af136b83b202
add left workarea

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