SEKIkikou.cpp

00001 
00002 #include "SEKIkikou.h"
00003 
00004 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){
00005     init(ik1, ik2, ik3, b1, b2, b3, b4, env1, env2, env3);
00006 }
00007 
00008 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){
00009     ik[0] = ik1; //射出
00010     ik[1] = ik2; //将校
00011     ik[2] = ik3; //装填
00012     button[0] = b1; //射出
00013     button[1] = b2; //上
00014     button[2] = b3; //下
00015     button[3] = b4; //装填
00016     encoder[0] = env1; //射出
00017     encoder[1] = env2; //将校
00018     encoder[2] = env3; //装填
00019     envBuff = 0;
00020     for(int i=0; i<3; i++){
00021         encoderAngle[i] = 360;
00022         mspeed[i] = 0;
00023         if(i < 2) buff[i] = 0;
00024     }
00025     injecterFlag = false;
00026     liftFlag = false;
00027 }
00028 
00029 void SEKIkikou::runAll(float speedI, float speedLoad, float speedLift, int angle)
00030 {
00031     runInjection(speedI);
00032     runLoad(speedLoad);
00033     runLift(speedLift, angle);
00034 }
00035 
00036 void SEKIkikou::stopAll(){
00037     //init(NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
00038     for(int i=0; i<4; i++){
00039         button[i] = NULL;
00040         if(i < 3){
00041             encoder[i] = NULL;
00042             mspeed[i] = 0;
00043         }
00044     }
00045 }
00046 
00047 void SEKIkikou::runInjection(float speed)
00048 {
00049     ik[0]->setSpeed(mspeed[0]);
00050     if(timer.read() >= 1){
00051         encoderAngle[0] += 360;
00052         timer.reset();
00053     }else{
00054         mspeed[0] = 0;
00055     }
00056     if(*button[0] <= 0 && !injecterFlag || *button[0] == NULL){
00057         mspeed[0] = 0;
00058         return;
00059     }
00060     if(*encoder[0] < encoderAngle[0]){
00061         mspeed[0] = speed;
00062         injecterFlag = true;
00063     }else{
00064         timer.start();
00065         mspeed[0] = 0;
00066         injecterFlag = false;
00067     }
00068 }
00069 
00070 void SEKIkikou::runLoad(float speed)
00071 {
00072     ik[2]->setSpeed(mspeed[1]);
00073     if(*button[1] <= 0 && *button[2] <= 0 || button[1] == NULL || button[2] == NULL){
00074         mspeed[1] = 0;
00075         return;   
00076     }else if(*button[1] >= 1){
00077         mspeed[1] = speed;
00078     }else if(*button[2] >= 1){
00079         mspeed[1] = -speed;
00080     }else{
00081         mspeed[1] = 0;   
00082     }
00083 }
00084 
00085 void SEKIkikou::runLift(float speed, int angle)
00086 {      
00087     ik[1]->setSpeed(mspeed[2]);
00088     
00089     buff[0] = *button[3] - buff[1];
00090     buff[1] = *button[3];
00091     
00092     if(liftFlag && *encoder[2] < envBuff + angle){
00093         mspeed[2] = speed;
00094     }else{
00095         liftFlag = false;
00096         mspeed[2] = 0;   
00097     }
00098     
00099     if((buff[0] <= 0 && !liftFlag) || button[3] == NULL){
00100         mspeed[2] = 0;
00101         return;   
00102     }
00103     if(buff[0] >= 1){
00104         liftFlag = true;
00105         envBuff = *encoder[2];
00106     }
00107 }