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@20:13934809e117, 2018-08-27 (annotated)

Committer:
shimizuta
Date:
Mon Aug 27 03:47:21 2018 +0000
Revision:
20:13934809e117
Parent:
19:48c3af917932
Child:
21:af9a33d69745
There are a lot of change

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 3:05b1dcb3634e 7
shimizuta 19:48c3af917932 8 //////////////////////////////変更できるパラメータ
shimizuta 19:48c3af917932 9 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
shimizuta 12:174f0090aa79 10 const double kOriginTheta[] = {
shimizuta 20:13934809e117 11 180,
shimizuta 20:13934809e117 12 180,
shimizuta 20:13934809e117 13 135,
shimizuta 12:174f0090aa79 14 205.875,
shimizuta 12:174f0090aa79 15 };//初期状態の角度
shimizuta 16:b2358fd35999 16 const int kServoSpan_ms = 3; //指示の前後に必要なwait
shimizuta 20:13934809e117 17 const double kOpenVal = 203.61375;//ハンドを開いたときのサーボへの指示
shimizuta 20:13934809e117 18 const double kCloseVal = 136;//ハンドを閉じたときのサーボへの指示
shimizuta 20:13934809e117 19 const int kTipServoID = 4;
shimizuta 19:48c3af917932 20 //////////////////////////////
shimizuta 20:13934809e117 21 KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:0
shimizuta 20:13934809e117 22 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:1
shimizuta 20:13934809e117 23 KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:2,3,4
shimizuta 20:13934809e117 24 };
shimizuta 20:13934809e117 25 void ServoRad(int id, double rad_relative)
shimizuta 19:48c3af917932 26 {
shimizuta 20:13934809e117 27 double degree = kServoSign[id] * rad_relative *kRadToDegree + kOriginTheta[id];
shimizuta 20:13934809e117 28 switch(id) {
shimizuta 20:13934809e117 29 case 0:
shimizuta 20:13934809e117 30 servo[1].set_degree(id, degree);
shimizuta 20:13934809e117 31 break;
shimizuta 20:13934809e117 32 case 1:
shimizuta 20:13934809e117 33 servo[2].set_degree(id, degree);
shimizuta 20:13934809e117 34 break;
shimizuta 20:13934809e117 35 case 2:
shimizuta 20:13934809e117 36 case 3:
shimizuta 20:13934809e117 37 case 4:
shimizuta 20:13934809e117 38 servo[0].set_degree(id, degree);
shimizuta 20:13934809e117 39 break;
shimizuta 20:13934809e117 40 }
shimizuta 20:13934809e117 41 SetNowRadRelative(id, rad_relative);
shimizuta 13:126c3f7f9b89 42 }
shimizuta 19:48c3af917932 43 void ServoMoveOnArmInTurns()
shimizuta 19:48c3af917932 44 {
shimizuta 20:13934809e117 45 static int count = 0;
shimizuta 20:13934809e117 46 double rad = GetNextRadRelative(count);
shimizuta 20:13934809e117 47 ServoRad(count, rad);
shimizuta 20:13934809e117 48 ++count;
shimizuta 20:13934809e117 49 if(count > SERVONUM - 2) count = 0;
shimizuta 19:48c3af917932 50 }
shimizuta 4:187c62291654 51 void ServoMoveOnArm()
shimizuta 3:05b1dcb3634e 52 {
shimizuta 19:48c3af917932 53 for(int i = 0; i < SERVONUM - 1; i++) {
shimizuta 19:48c3af917932 54 ServoMoveOnArmInTurns();
shimizuta 3:05b1dcb3634e 55 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 56 }
shimizuta 3:05b1dcb3634e 57 }
shimizuta 3:05b1dcb3634e 58 void Open()
shimizuta 3:05b1dcb3634e 59 {
shimizuta 20:13934809e117 60 servo[2].set_degree(kTipServoID, kOpenVal);
shimizuta 3:05b1dcb3634e 61 }
shimizuta 2:4515e8cc6b60 62 void Close()
shimizuta 2:4515e8cc6b60 63 {
shimizuta 20:13934809e117 64 servo[2].set_degree(kTipServoID, kCloseVal);
shimizuta 5:af5ccfce1b90 65 }