File content as of revision 71:7d684ff43ddd:

//index_include
#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 "pid_encoder.h"
#include "cyclic_var.h"
#include "cyclic_io.h"
#include "cylinder.h"
#include "event_var.h"
//#include "servo.h"


//index_define
#define SBDBT_TX    p13
#define SBDBT_RX    p14
#define SBDBT_STO p12
#define RS422_TX    p28
#define RS422_RX    p27
#define CONTROL_INDICATOR p11

namespace BAUD {
    const int PC    = 460800;
    const int SBDBT = 115200;
    const int RS422 = 115200;
}

namespace CYCLE {
    const float OUTPUT  = 0.015;
}

namespace MECANUM {
    const float ACCELERATION = 15;
}

namespace YAW {
    const float KP = 0.01;
    const float KI = 0.01;
    const float KD = 0.01;
}

namespace SHOT {
    const float ENCORDER_KP = 0.0400;
    const float ENCORDER_KI = 0.0001;
    const float ENCORDER_KD = 0.0003;
    const int CYLINDER_POSITION_MAX = 6;
}


#define nucleo_num  6
#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 SHOT_CYLINDER_ON     p18
#define SHOT_CYLINDER_OFF    p17
#define SHOT_INTERRUPT       p29
#define SHOT_ENCODER_A       p21
#define SHOT_ENCODER_B       p22
#define RELOAD_CYLINDER p15

#define mecanum_power   1.0
#define sword_power     1.0
#define sholder_power   1.0
#define SHOULDER_RIGHT_INTERRUPT_MAX  p19 //p21
#define SHOULDER_RIGHT_INTERRUPT_MIN  p20 //p22
#define SHOULDER_LEFT_INTERRUPT_MAX   p7
#define SHOULDER_LEFT_INTERRUPT_MIN   p8
//#define pin_servo p21
//#define servo_reload_time 1.0


//index_setupPin
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalIn interrupt_cylinder_min(SHOT_INTERRUPT);
Serial pc(USBTX,USBRX);
Rs422 rs422(RS422_TX, RS422_RX);
Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
Ticker output_timer;
Mecanum mecanum;
Bno055 bno055;
PositionPid yaw_pid;
Accelerator v1;
Accelerator v2;
Accelerator v3;
Accelerator v4;
DigitalOut cylinder_on(SHOT_CYLINDER_ON);
DigitalOut cylinder_off(SHOT_CYLINDER_OFF);
CyclicVar sword;
CyclicVar cyclic_cylinder_position;
DigitalIn interrupt_sholderright_min(SHOULDER_RIGHT_INTERRUPT_MIN);
DigitalIn interrupt_sholderright_max(SHOULDER_RIGHT_INTERRUPT_MAX);
DigitalIn interrupt_sholderleft_min(SHOULDER_LEFT_INTERRUPT_MIN);
DigitalIn interrupt_sholderleft_max(SHOULDER_LEFT_INTERRUPT_MAX);
Encoder enc_cylinder(SHOT_ENCODER_A,SHOT_ENCODER_B);
CyclicVar cyclic_servo;
CyclicIo cylinder_reload(RELOAD_CYLINDER);
//PwmOut servo(pin_servo);
DigitalOut sbdbt_indigator(CONTROL_INDICATOR);
eventVar cylinder_event;


//index_setupFunction
void setup();
void output();
void motor_cal();
void cylinder_cal();
void boost();
void cylinder_origin();
void firstmotion();
void sword_cal();
void servo_origin();
float shoulder_right_cal();
float shoulder_left_cal();

//index_setupVariable
//output
float yaw, target_yaw;
//cylinder_origin
int cylinder_origin_flag = 0;
//cylinder
float cylinder_pwm;
int cylinder_pos_num = 0;
float cylinder_pos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0};
float cylinder_power = 0.0;

int main()
{
    setup();
    while(1) {
        led1 = interrupt_sholderleft_max;
        led2 = interrupt_sholderleft_min;
        led3 = interrupt_sholderright_max;
        led4 = interrupt_sholderright_min;
        
        //pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
        //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
    }
}

void setup()
{
    wait(2.0);
    bno055.begin();
    wait(1.0);
    bno055.firstRead();
    pc.baud(BAUD::PC);
    sbdbt.begin(BAUD::SBDBT);
    rs422.begin(BAUD::RS422);
    firstmotion();
    cylinder_pos_num = 2;                   //セットアップタイムでの初期装填のため
    output_timer.attach(&output, CYCLE::OUTPUT);
    yaw_pid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT);
    mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
    v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    enc_cylinder.setPpr(100);
    enc_cylinder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT);
    //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 firstmotion()
{
        float cylinder = 0.0;
        float sholderL = 0.0;
        float sholderR = 0.0;
    while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){
        if(interrupt_cylinder_min == 1){
            cylinder = 2.0;
        }else{
            cylinder = 0.0;
        }
        if(interrupt_sholderright_max == 1){
            sholderR = -2.0;
        }else{
            sholderR = 0.0;
        }
        if(interrupt_sholderleft_max == 1){
            sholderL = 2.0;
        }else{
            sholderL = 0.0;
        }
            rs422.put(5, cylinder, 0.0);
            rs422.put(6, sholderR, sholderL);
    }
    enc_cylinder.origin();
}

//cylinder_origin_flagを1にすることで動作する
void cylinder_origin()
{
}
float cylinder_power_cal(){
        if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
            cylinder_origin_flag = 0;
            enc_cylinder.origin();
            cylinder_pos_num = 0;
            cylinder_power = -0.0;
        }else if(cylinder_origin_flag == 1){
            return -0.8;
        }else{
            return -1*enc_cylinder.duty_enableWidth(-10.0,10.0);   
        }
}

void cylinder_cal()
{
    //cylinder ON/OFF
    if(sbdbt.shikaku){
        cylinder_on = 1;
        cylinder_off = 0;
    }else{
        cylinder_on = 0;
        cylinder_off = 1;    
    }
    
    cylinder_event.input(sbdbt.right);
    if(cylinder_event.getRise()) {        //cylinder degset
        cylinder_pos_num++;
        if(cylinder_pos_num >= SHOT::CYLINDER_POSITION_MAX) {
            //cylinder_pos_num = 0;
            cylinder_origin_flag=1;
        }
    }
    enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],CYCLE::OUTPUT); //set cylinder_tergetPos

    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());
}

//function
/*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();
    }
}*/


//functionBoost
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();
    }
    */
}

//functionMecanum
void motor_cal()
{
    static float out_right_x;
    yaw = bno055.getYawRad();
    target_yaw = yaw;
    yaw_pid.calculate(target_yaw, yaw); 
    
    //後進時に方向が逆転するため
    if(sbdbt.right_y < 0.0){
        out_right_x = -1.0*sbdbt.right_x;
    }else{
        out_right_x = sbdbt.right_x;
    }
    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, 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();
    sbdbt_indigator = sbdbt.get_pairingState();
    //led1 = sbdbt.get_pairingState();
    if(sbdbt.up)cylinder_origin_flag = 1;
    if(sbdbt.left)bno055.yaw_origin();
    cylinder_reload.cyclic(sbdbt.down);

    static int counter;
    static float mecanumV1, mecanumV2, mecanumV3, mecanumV4;
    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(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)));
                counter++;
                break;
            case 1:
                //.duty(<cal>*<powerControle>+(<boost>*0.5))
                rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)));
                counter ++;
                break;
            case 2:
                //rs422.put(id[counter], sbdbt.right_y, 0.0);
                rs422.put(id[counter], limitf(cylinder_power_cal(),1.0,-1.0), 0.0);
                counter ++;
                break;
            case 3:
                rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0));
                counter ++;
                break;
            case 4:
                rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0);
                counter = 0;
                break;
            default:
                break;
        }
    }else{
        switch (counter) {
            case 0:
                rs422.put(id[counter],0.0,0.0);
                counter++;
                break;
            case 1:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            case 2:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            case 3:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            case 4:
                rs422.put(id[counter],0.0,0.0);
                counter = 0;
                break;
            default:
                break;
        }
    }
}