yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

Committer:
shimizuta
Date:
Sat Aug 11 07:21:59 2018 +0000
Revision:
10:6d1d44fa9739
Parent:
5:af5ccfce1b90
Child:
12:174f0090aa79
add exact kArmLength

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 10:6d1d44fa9739 9 const double kOriginTheta[] = {164,236.8669,151.18,180}; //初期状態の角度
shimizuta 10:6d1d44fa9739 10 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
shimizuta 3:05b1dcb3634e 11 const int kServoSpan_ms = 5; //指示の前後に必要なwait
shimizuta 0:c3a72fe24137 12
shimizuta 2:4515e8cc6b60 13 void Open();
shimizuta 3:05b1dcb3634e 14 void Move(double x, double y, double theta);
shimizuta 3:05b1dcb3634e 15
shimizuta 4:187c62291654 16 void ServoMoveOnArm()
shimizuta 3:05b1dcb3634e 17 {
shimizuta 4:187c62291654 18 for(int i = 0; i < kServoNum; i++) {
shimizuta 5:af5ccfce1b90 19 double rad = GetNextRadRelative(i);
shimizuta 10:6d1d44fa9739 20 servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
shimizuta 5:af5ccfce1b90 21 SetNowRadRelative(i,rad);
shimizuta 3:05b1dcb3634e 22 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 23 }
shimizuta 5:af5ccfce1b90 24
shimizuta 3:05b1dcb3634e 25 }
shimizuta 2:4515e8cc6b60 26
shimizuta 2:4515e8cc6b60 27 int HandSet(WorkState &params)
shimizuta 1:cc056c41ae30 28 {
shimizuta 2:4515e8cc6b60 29 switch(params.areaname) {
shimizuta 2:4515e8cc6b60 30 case COMMONAREA:
shimizuta 2:4515e8cc6b60 31 case WORKAREA:
shimizuta 2:4515e8cc6b60 32 Open();
shimizuta 2:4515e8cc6b60 33 break;
shimizuta 2:4515e8cc6b60 34 case BOX:
shimizuta 4:187c62291654 35 Close();
shimizuta 2:4515e8cc6b60 36 break;
shimizuta 2:4515e8cc6b60 37 }
shimizuta 2:4515e8cc6b60 38 return 1;
shimizuta 2:4515e8cc6b60 39 }
shimizuta 2:4515e8cc6b60 40
shimizuta 3:05b1dcb3634e 41 void Open()
shimizuta 3:05b1dcb3634e 42 {
shimizuta 3:05b1dcb3634e 43 servo.set_degree(4,182.7225);
shimizuta 3:05b1dcb3634e 44 }
shimizuta 2:4515e8cc6b60 45
shimizuta 2:4515e8cc6b60 46 void Close()
shimizuta 2:4515e8cc6b60 47 {
shimizuta 3:05b1dcb3634e 48 servo.set_degree(4,135.43875);
shimizuta 5:af5ccfce1b90 49 }