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

Committer:
shimizuta
Date:
2018-08-09
Revision:
5:af5ccfce1b90
Parent:
4:187c62291654
Child:
9:a553ace4d9d1
Child:
10:6d1d44fa9739

File content as of revision 5:af5ccfce1b90:

#include "servo.h"
#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "pinnames.h"
#include "coordinate.h"
#include "KondoServo.h"

KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
const double kOriginTheta[] = {180,180,180,180}; //初期状態の角度
const int kServoSign[] = {-1, -1, 1, 1};//サーボの正負と座標系の正負の補正
const int kServoSpan_ms = 5; //指示の前後に必要なwait

void Open();
void Move(double x, double y, double theta);

void ServoMoveOnArm()
{
    for(int i = 0; i < kServoNum; i++) {
        double rad = GetNextRadRelative(i);
        servo.set_degree(i, kServoSign[i] * rad + kOriginTheta[i]);
        SetNowRadRelative(i,rad);
        wait_ms(kServoSpan_ms);
    }
    
}

int HandSet(WorkState &params)
{
    switch(params.areaname) {
        case COMMONAREA:
        case WORKAREA:
            Open();
            break;
        case BOX:
            Close();
            break;
    }
    return 1;
}

void Open()
{
    servo.set_degree(4,182.7225);
}

void Close()
{
    servo.set_degree(4,135.43875);
}