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@4:187c62291654, 2018-08-09 (annotated)

Committer:
shimizuta
Date:
Thu Aug 09 05:37:04 2018 +0000
Revision:
4:187c62291654
Parent:
3:05b1dcb3634e
Child:
5:af5ccfce1b90
servo ver

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 4:187c62291654 9 const double kOriginTheta[] = {180,180,180,180}; //初期状態の角度
shimizuta 4:187c62291654 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 4:187c62291654 19 servo.set_degree(i, kServoSign[i]*GetNextRadRelative(i)+kOriginTheta[i]);
shimizuta 3:05b1dcb3634e 20 wait_ms(kServoSpan_ms);
shimizuta 3:05b1dcb3634e 21 }
shimizuta 3:05b1dcb3634e 22 }
shimizuta 2:4515e8cc6b60 23
shimizuta 4:187c62291654 24
shimizuta 4:187c62291654 25
shimizuta 2:4515e8cc6b60 26 int HandSet(WorkState &params)
shimizuta 1:cc056c41ae30 27 {
shimizuta 2:4515e8cc6b60 28 switch(params.areaname) {
shimizuta 2:4515e8cc6b60 29 case COMMONAREA:
shimizuta 2:4515e8cc6b60 30 case WORKAREA:
shimizuta 2:4515e8cc6b60 31 Open();
shimizuta 2:4515e8cc6b60 32 break;
shimizuta 2:4515e8cc6b60 33 case BOX:
shimizuta 4:187c62291654 34 Close();
shimizuta 2:4515e8cc6b60 35 break;
shimizuta 2:4515e8cc6b60 36 }
shimizuta 2:4515e8cc6b60 37 return 1;
shimizuta 2:4515e8cc6b60 38 }
shimizuta 2:4515e8cc6b60 39
shimizuta 3:05b1dcb3634e 40 void Open()
shimizuta 3:05b1dcb3634e 41 {
shimizuta 3:05b1dcb3634e 42 servo.set_degree(4,182.7225);
shimizuta 3:05b1dcb3634e 43 }
shimizuta 2:4515e8cc6b60 44
shimizuta 2:4515e8cc6b60 45 void Close()
shimizuta 2:4515e8cc6b60 46 {
shimizuta 3:05b1dcb3634e 47 servo.set_degree(4,135.43875);
shimizuta 2:4515e8cc6b60 48 }
shimizuta 3:05b1dcb3634e 49
shimizuta 2:4515e8cc6b60 50 void ArmHorizon()
shimizuta 2:4515e8cc6b60 51 {
shimizuta 3:05b1dcb3634e 52 servo.set_degree(3,172.125);
shimizuta 3:05b1dcb3634e 53 SetArmState(HORIZON);
shimizuta 3:05b1dcb3634e 54 }
shimizuta 2:4515e8cc6b60 55 void ArmVertical()
shimizuta 2:4515e8cc6b60 56 {
shimizuta 3:05b1dcb3634e 57 servo.set_degree(3,262.125);
shimizuta 2:4515e8cc6b60 58 }
shimizuta 4:187c62291654 59 /*
shimizuta 3:05b1dcb3634e 60 void ArmVerticalForward()
shimizuta 2:4515e8cc6b60 61 {
shimizuta 3:05b1dcb3634e 62 ArmVertical();
shimizuta 3:05b1dcb3634e 63 SetArmState(VERTICAL_FORWARD);
shimizuta 2:4515e8cc6b60 64 }
shimizuta 3:05b1dcb3634e 65 void ArmVerticalBack()
shimizuta 2:4515e8cc6b60 66 {
shimizuta 3:05b1dcb3634e 67 ArmVertical();
shimizuta 3:05b1dcb3634e 68 SetArmState(VERTICAL_BACK);
shimizuta 2:4515e8cc6b60 69 }
shimizuta 4:187c62291654 70 */