File content as of revision 45:a32e8091901b:

#include "mbed.h"
#include "math.h"
#include "bit_test.h"
#include "RS422_put.h"
#include "sbdbt.h"
#include "mecanum.h"
#include "bno055_lib.h"
#include "bno055_use.h"
#include "pid.h"
#include "limit.h"
#include "accelerator.h"
#include "encorder.h"
#include "cyclic.h"
#include "cyclic_IO.h"
#include "cylinder.h"


#define pc_baud     460800
#define sbdbt_tx    p13
#define sbdbt_rx    p14
#define sbdbt_baud  115200
#define rs422_tx    p28
#define rs422_rx    p27
#define rs422_baud  115200
#define output_period   0.015
#define nucleo_num  6
#define pi          3.141592
#define n1_id       3
#define n2_id       4
#define n3_id       5
#define n4_id       6
#define n5_id       7
#define n6_id       8
#define yaw_Kp      0.01
#define yaw_Ki      0.01
#define yaw_Kd      0.01
#define acceleration    20  //25
#define pin_cylinder_on     p17
#define pin_cylinder_off    p18
#define pin_interrupt_cylinder_min       p23
#define encoder_A       p25
#define encoder_B       p26
#define enc_Kp      0.0400
#define enc_Ki      0.0001
#define enc_Kd      0.0003
#define mecanum_power   1.0
#define sword_power     0.8
#define sholder_power   0.8
#define pin_interrupt_sholderright_max  p11 //p21
#define pin_interrupt_sholderright_min  p12 //p22
#define pin_interrupt_sholderleft_max   p7
#define pin_interrupt_sholderleft_min   p8
#define pin_servo p21
#define servo_reload_time 1.0
#define pin_cylinder_reload p15
#define pin_sbdbt_pairing p12
#define pin_sbdbt_indicator p11
//#define pin_servo_reload p29 //
//#define pin_interrupt_reload p30 //

DigitalOut led1(LED1);
//DigitalOut led2(LED2);
//DigitalOut led3(LED3);
//DigitalOut led4(LED4);
DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
Serial pc(USBTX,USBRX);
RS422 rs422(rs422_tx, rs422_rx);
Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing);
Ticker output_timer;
Mecanum mecanum;
Bno055 bno055;
Position_pid yaw_pid;
Accel v1;
Accel v2;
Accel v3;
Accel v4;
Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
Cyclic sword;
Cyclic cyclic_cylinder_position;
DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
Encoder enc_cylinder(encoder_A,encoder_B);
Cyclic cyclic_servo;
Cyclic_IO cylinder_reload(pin_cylinder_reload);

//追加
PwmOut servo(pin_servo);
Timeout timer_servo;
DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
//Servo servo_reload(pin_servo_reload);
//DigitalIn interrupt_reload(pin_interrupt_reload);

void setup();
void output();
void motor_cal();
void cylinder_cal();
void boost();
void cylinder_origin();
void cylinder_origin_first();
void sword_cal();
void servo_origin();
float shoulder_right_cal();
float shoulder_left_cal();

//output
float yaw, target_yaw;
//cylinder_origin
int cylinder_origin_flag = 0;
//cylinder
float cylinder_pwm;
int cylinder_pos_num = 0;
float cylinder_pos[3] = {0.0,90.0,325.0};

//riseEventそのうちClassにしたい
short state;
int riseEvent(int input)
{
    state = ((state<<1)|input)&3;
    if(state == 1) {
        return 1;
    } else {
        return 0;
    }
}

int main()
{
    setup();
    while(1) {
//        led1 = interrupt_sholderleft_max;
//        led2 = interrupt_sholderleft_min;
//        led3 = interrupt_sholderright_max;
//        led4 = interrupt_sholderright_min;
        //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
        //pc.printf("rise state : %d\r\n",cyclic_servo.getState());
        pc.printf("Sbdbtstate : %d\r\n",sbdbt.get_pairingState());
    }
}

void setup()
{
    wait(2.0);
    bno055.begin();
    wait(1.0);
    bno055.firstRead();
    pc.baud(pc_baud);
    sbdbt.begin(sbdbt_baud);
    rs422.begin(rs422_baud);
    cylinder_origin_first();
    cylinder_pos_num = 1;                   //セットアップタイムでの初期装填のため
    output_timer.attach(&output, output_period);
    yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
    mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
    v1.setup(acceleration,output_period);
    v2.setup(acceleration,output_period);
    v3.setup(acceleration,output_period);
    v4.setup(acceleration,output_period);
    enc_cylinder.setup(100);
    enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);

    //追加
    servo.period(0.020);
}

//Sword
float shoulder_right_cal()
{
    if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) {
        return 0.0;
    }
    if(interrupt_sholderright_min==0&&sbdbt.batu==1) {
        return 0.0;
    }
    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
}

float shoulder_left_cal()
{
    if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) {
        return 0.0;
    }
    if(interrupt_sholderleft_min==0&&sbdbt.batu==1) {
        return 0.0;
    }
    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
}
void sword_cal()
{
    sword.cyclic(sbdbt.maru);
}

//cylinder
void cylinder_origin_first()
{
    while(interrupt_cylinder_min == 1) {
        rs422.put(5, -0.4, 0.0);
    }
    enc_cylinder.origin();
}

//cylinder_origin_flagを1にすることで動作する
void cylinder_origin()
{
    if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
        cylinder_origin_flag = 0;
        enc_cylinder.origin();
        cylinder_pos_num = 0;
    } else if(cylinder_origin_flag == 1) {
        rs422.put(5, -0.8, 0.0);
    }
}

//追記(動作未確認)
void cylinder_cal()
{
    cylinder.cyclic(sbdbt.shikaku);     //cylinder ON/OFF

    if(riseEvent(sbdbt.right)) {        //cylinder degset
        cylinder_pos_num++;
        if(cylinder_pos_num >= 3) {
            //cylinder_pos_num = 0;
            cylinder_origin_flag=1;
        }
    }
    enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定

    //pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());

//リロード機構完成後
    /*
        if(cylinder.getInState() == 1){
            if(interrupt_cylinder_min)servo_reload = 1.0;
        }else{
            servo_reload = 0.0;
            reload_magazine_flag = 1;
        }

        if(reload_magazine_flag == 1){
            rs422.put(n6_id, 0.8, 0.0);
            if(interrupt_reload == 1)reload_magazine_flag = 0;
        }
    */
}

void servo_max()
{
    servo.pulsewidth(0.0022);
}
void servo_min()
{
    servo.pulsewidth(0.0010);
}
void servo_out()
{
    cyclic_servo.cyclic(sbdbt.down);    //setServoControl
    if(cyclic_servo.getState()==0) {
        servo_min();
    } else if(cyclic_servo.getState()==1) {
        servo_max();
    }
}


//boost
void boost()
{
    if(sbdbt.r2) {
        mecanum.boost_forward();
    }
    if(sbdbt.l2) {
        mecanum.boost_back();
    }
    /*
    if(sbdbt.shikaku) {
        mecanum.boost_left();
    }
    if(sbdbt.maru) {
        mecanum.boost_right();
    }
    */
}

//mecanum
void motor_cal()
{
    yaw = bno055.getYawRad();
    target_yaw = yaw;
    yaw_pid.cal(target_yaw, yaw, output_period);
    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
//    pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY());
}
void output()
{
    motor_cal();
    cylinder_cal();
    sword_cal();
    servo_out();
    cylinder_origin();
    //boost();

    led1 = sbdbt.get_pairingState();
    sbdbt_indigator = sbdbt.get_pairingState();

    if(sbdbt.up) {
        cylinder_origin_flag = 1;
    }

    cylinder_reload.cyclic(sbdbt.left);

    static int counter;
    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};

    //sbdbtがpairingしている場合のみ動作
    if(sbdbt.get_pairingState()) {
        switch (counter) {
            case 0:
                rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5)));
                counter++;
                break;
            case 1:
                //.duty(<cal>*<powerControle>+(<boost>*0.5))
                rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.5)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.5)));
                counter ++;
                break;
            case 2:
                rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0);
                counter ++;
                break;
            case 3:
                rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power);
                counter ++;
                break;
            case 4:
                rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
                counter = 0;
                break;
            default:
                break;
        };
    }else{
        for(int count_i = 0;count_i<=nucleo_num;count_i++){
            rs422.put(id[count_i],0.0,0.0);
        }
    }
}