File content as of revision 11:9164aa147df1:

#include "SEKIkikou.h"

SEKIkikou::SEKIkikou(ikarashiMDC *ik1, ikarashiMDC *ik2, ikarashiMDC *ik3, uint8_t *b1, uint8_t *b2, uint8_t *b3, uint8_t *b4, int *env1, int *env2, int *env3){
    init(ik1, ik2, ik3, b1, b2, b3, b4, env1, env2, env3);
}

void SEKIkikou::init(ikarashiMDC *ik1, ikarashiMDC *ik2, ikarashiMDC *ik3, uint8_t *b1, uint8_t *b2, uint8_t *b3, uint8_t *b4, int *env1, int *env2, int *env3){
    ik[0] = ik1; //射出
    ik[1] = ik2; //将校
    ik[2] = ik3; //装填
    button[0] = b1; //射出
    button[1] = b2; //上
    button[2] = b3; //下
    button[3] = b4; //装填
    encoder[0] = env1; //射出
    encoder[1] = env2; //将校
    encoder[2] = env3; //装填
    envBuff = 0;
    for(int i=0; i<3; i++){
        encoderAngle[i] = 360;
        mspeed[i] = 0;
        if(i < 2) buff[i] = 0;
    }
    injecterFlag = false;
    liftFlag = false;
}

void SEKIkikou::runAll(float speedI, float speedLoad, float speedLift, int angle)
{
    runInjection(speedI);
    runLoad(speedLoad);
    runLift(speedLift, angle);
}

void SEKIkikou::stopAll(){
    //init(NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
    for(int i=0; i<4; i++){
        button[i] = NULL;
        if(i < 3){
            encoder[i] = NULL;
            mspeed[i] = 0;
        }
    }
}

void SEKIkikou::runInjection(float speed)
{
    ik[0]->setSpeed(mspeed[0]);
    if(timer.read() >= 1){
        encoderAngle[0] += 360;
        timer.reset();
    }else{
        mspeed[0] = 0;
    }
    if(*button[0] <= 0 && !injecterFlag || *button[0] == NULL){
        mspeed[0] = 0;
        return;
    }
    if(*encoder[0] < encoderAngle[0]){
        mspeed[0] = speed;
        injecterFlag = true;
    }else{
        timer.start();
        mspeed[0] = 0;
        injecterFlag = false;
    }
}

void SEKIkikou::runLoad(float speed)
{
    ik[2]->setSpeed(mspeed[1]);
    if(*button[1] <= 0 && *button[2] <= 0 || button[1] == NULL || button[2] == NULL){
        mspeed[1] = 0;
        return;   
    }else if(*button[1] >= 1){
        mspeed[1] = speed;
    }else if(*button[2] >= 1){
        mspeed[1] = -speed;
    }else{
        mspeed[1] = 0;   
    }
}

void SEKIkikou::runLift(float speed, int angle)
{      
    ik[1]->setSpeed(mspeed[2]);
    
    buff[0] = *button[3] - buff[1];
    buff[1] = *button[3];
    
    if(liftFlag && *encoder[2] < envBuff + angle){
        mspeed[2] = speed;
    }else{
        liftFlag = false;
        mspeed[2] = 0;   
    }
    
    if((buff[0] <= 0 && !liftFlag) || button[3] == NULL){
        mspeed[2] = 0;
        return;   
    }
    if(buff[0] >= 1){
        liftFlag = true;
        envBuff = *encoder[2];
    }
}