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@13:126c3f7f9b89, 2018-08-13 (annotated)

Committer:
shimizuta
Date:
Mon Aug 13 09:03:30 2018 +0000
Revision:
13:126c3f7f9b89
Parent:
12:174f0090aa79
Child:
16:b2358fd35999
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 13:126c3f7f9b89 18 const int kServoSpan_ms = 2; //指示の前後に必要なwait
shimizuta 0:c3a72fe24137 19
shimizuta 2:4515e8cc6b60 20 void Open();
shimizuta 3:05b1dcb3634e 21 void Move(double x, double y, double theta);
shimizuta 13:126c3f7f9b89 22 void ServoRad(int i, double rad){
shimizuta 13:126c3f7f9b89 23 servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
shimizuta 13:126c3f7f9b89 24 }
shimizuta 4:187c62291654 25 void ServoMoveOnArm()
shimizuta 3:05b1dcb3634e 26 {
shimizuta 4:187c62291654 27 for(int i = 0; i < kServoNum; i++) {
shimizuta 5:af5ccfce1b90 28 double rad = GetNextRadRelative(i);
shimizuta 13:126c3f7f9b89 29 ServoRad(i, rad);
shimizuta 5:af5ccfce1b90 30 SetNowRadRelative(i,rad);
shimizuta 3:05b1dcb3634e 31 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 32 }
shimizuta 12:174f0090aa79 33
shimizuta 3:05b1dcb3634e 34 }
shimizuta 2:4515e8cc6b60 35
shimizuta 2:4515e8cc6b60 36 int HandSet(WorkState &params)
shimizuta 1:cc056c41ae30 37 {
shimizuta 2:4515e8cc6b60 38 switch(params.areaname) {
shimizuta 2:4515e8cc6b60 39 case COMMONAREA:
shimizuta 2:4515e8cc6b60 40 case WORKAREA:
shimizuta 2:4515e8cc6b60 41 Open();
shimizuta 2:4515e8cc6b60 42 break;
shimizuta 2:4515e8cc6b60 43 case BOX:
shimizuta 4:187c62291654 44 Close();
shimizuta 2:4515e8cc6b60 45 break;
shimizuta 2:4515e8cc6b60 46 }
shimizuta 2:4515e8cc6b60 47 return 1;
shimizuta 2:4515e8cc6b60 48 }
shimizuta 2:4515e8cc6b60 49
shimizuta 3:05b1dcb3634e 50 void Open()
shimizuta 3:05b1dcb3634e 51 {
shimizuta 12:174f0090aa79 52 servo.set_degree(4,203.61375);
shimizuta 3:05b1dcb3634e 53 }
shimizuta 2:4515e8cc6b60 54 void Close()
shimizuta 2:4515e8cc6b60 55 {
shimizuta 12:174f0090aa79 56 servo.set_degree(4,124.63875);
shimizuta 5:af5ccfce1b90 57 }