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@12:174f0090aa79, 2018-08-13 (annotated)

Committer:
shimizuta
Date:
Mon Aug 13 02:22:58 2018 +0000
Revision:
12:174f0090aa79
Parent:
10:6d1d44fa9739
Child:
13:126c3f7f9b89
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 3:05b1dcb3634e 7
shimizuta 3:05b1dcb3634e 8 KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
shimizuta 12:174f0090aa79 9 const double kOriginTheta[] = {
shimizuta 12:174f0090aa79 10 163.9575,
shimizuta 12:174f0090aa79 11 236.79,
shimizuta 12:174f0090aa79 12 151.1325,
shimizuta 12:174f0090aa79 13 205.875,
shimizuta 12:174f0090aa79 14 };//初期状態の角度
shimizuta 12:174f0090aa79 15
shimizuta 12:174f0090aa79 16
shimizuta 10:6d1d44fa9739 17 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
shimizuta 3:05b1dcb3634e 18 const int kServoSpan_ms = 5; //指示の前後に必要なwait
shimizuta 0:c3a72fe24137 19
shimizuta 2:4515e8cc6b60 20 void Open();
shimizuta 3:05b1dcb3634e 21 void Move(double x, double y, double theta);
shimizuta 3:05b1dcb3634e 22
shimizuta 4:187c62291654 23 void ServoMoveOnArm()
shimizuta 3:05b1dcb3634e 24 {
shimizuta 4:187c62291654 25 for(int i = 0; i < kServoNum; i++) {
shimizuta 5:af5ccfce1b90 26 double rad = GetNextRadRelative(i);
shimizuta 10:6d1d44fa9739 27 servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
shimizuta 5:af5ccfce1b90 28 SetNowRadRelative(i,rad);
shimizuta 3:05b1dcb3634e 29 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 30 }
shimizuta 12:174f0090aa79 31
shimizuta 3:05b1dcb3634e 32 }
shimizuta 2:4515e8cc6b60 33
shimizuta 2:4515e8cc6b60 34 int HandSet(WorkState &params)
shimizuta 1:cc056c41ae30 35 {
shimizuta 2:4515e8cc6b60 36 switch(params.areaname) {
shimizuta 2:4515e8cc6b60 37 case COMMONAREA:
shimizuta 2:4515e8cc6b60 38 case WORKAREA:
shimizuta 2:4515e8cc6b60 39 Open();
shimizuta 2:4515e8cc6b60 40 break;
shimizuta 2:4515e8cc6b60 41 case BOX:
shimizuta 4:187c62291654 42 Close();
shimizuta 2:4515e8cc6b60 43 break;
shimizuta 2:4515e8cc6b60 44 }
shimizuta 2:4515e8cc6b60 45 return 1;
shimizuta 2:4515e8cc6b60 46 }
shimizuta 2:4515e8cc6b60 47
shimizuta 3:05b1dcb3634e 48 void Open()
shimizuta 3:05b1dcb3634e 49 {
shimizuta 12:174f0090aa79 50 servo.set_degree(4,203.61375);
shimizuta 3:05b1dcb3634e 51 }
shimizuta 2:4515e8cc6b60 52 void Close()
shimizuta 2:4515e8cc6b60 53 {
shimizuta 12:174f0090aa79 54 servo.set_degree(4,124.63875);
shimizuta 5:af5ccfce1b90 55 }