yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
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?
User | Revision | Line number | New 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 ¶ms) |
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 | } |