NagaokaRoboticsClub_mbedTeam / SEKIkikou

I don't know if it work properly

Dependents:   2022_NHK_B_UK 2022_NHK_B_sub2

SEKIkikou.cpp@11:9164aa147df1, 2022-10-13 (annotated)

Committer:
ikarashikota
Date:
Thu Oct 13 07:27:46 2022 +0000
Revision:
11:9164aa147df1
Parent:
10:10c255272359 
fixed bug 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikarashikota 0:04c98c2cfe16 1
ikarashikota 0:04c98c2cfe16 2 #include "SEKIkikou.h"
ikarashikota 0:04c98c2cfe16 3
ikarashikota 0:04c98c2cfe16 4 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){
ikarashikota 1:fab774f63278 5 init(ik1, ik2, ik3, b1, b2, b3, b4, env1, env2, env3);
ikarashikota 1:fab774f63278 6 }
ikarashikota 1:fab774f63278 7
ikarashikota 1:fab774f63278 8 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){
ikarashikota 0:04c98c2cfe16 9 ik[0] = ik1; //射出
ikarashikota 0:04c98c2cfe16 10 ik[1] = ik2; //将校
ikarashikota 0:04c98c2cfe16 11 ik[2] = ik3; //装填
ikarashikota 0:04c98c2cfe16 12 button[0] = b1; //射出
ikarashikota 0:04c98c2cfe16 13 button[1] = b2; //上
ikarashikota 0:04c98c2cfe16 14 button[2] = b3; //下
ikarashikota 0:04c98c2cfe16 15 button[3] = b4; //装填
ikarashikota 0:04c98c2cfe16 16 encoder[0] = env1; //射出
ikarashikota 0:04c98c2cfe16 17 encoder[1] = env2; //将校
ikarashikota 0:04c98c2cfe16 18 encoder[2] = env3; //装填
ikarashikota 10:10c255272359 19 envBuff = 0;
ikarashikota 6:34f3cf5b05f8 20 for(int i=0; i<3; i++){
ikarashikota 6:34f3cf5b05f8 21 encoderAngle[i] = 360;
ikarashikota 6:34f3cf5b05f8 22 mspeed[i] = 0;
ikarashikota 10:10c255272359 23 if(i < 2) buff[i] = 0;
ikarashikota 6:34f3cf5b05f8 24 }
ikarashikota 3:e62282fc7cf6 25 injecterFlag = false;
ikarashikota 10:10c255272359 26 liftFlag = false;
ikarashikota 0:04c98c2cfe16 27 }
ikarashikota 0:04c98c2cfe16 28
ikarashikota 10:10c255272359 29 void SEKIkikou::runAll(float speedI, float speedLoad, float speedLift, int angle)
ikarashikota 0:04c98c2cfe16 30 {
ikarashikota 0:04c98c2cfe16 31 runInjection(speedI);
ikarashikota 0:04c98c2cfe16 32 runLoad(speedLoad);
ikarashikota 10:10c255272359 33 runLift(speedLift, angle);
ikarashikota 0:04c98c2cfe16 34 }
ikarashikota 0:04c98c2cfe16 35
ikarashikota 1:fab774f63278 36 void SEKIkikou::stopAll(){
ikarashikota 2:5306c227e18b 37 //init(NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
ikarashikota 2:5306c227e18b 38 for(int i=0; i<4; i++){
ikarashikota 2:5306c227e18b 39 button[i] = NULL;
ikarashikota 2:5306c227e18b 40 if(i < 3){
ikarashikota 2:5306c227e18b 41 encoder[i] = NULL;
ikarashikota 4:d3814c6e3694 42 mspeed[i] = 0;
ikarashikota 2:5306c227e18b 43 }
ikarashikota 2:5306c227e18b 44 }
ikarashikota 1:fab774f63278 45 }
ikarashikota 1:fab774f63278 46
ikarashikota 0:04c98c2cfe16 47 void SEKIkikou::runInjection(float speed)
ikarashikota 0:04c98c2cfe16 48 {
ikarashikota 0:04c98c2cfe16 49 ik[0]->setSpeed(mspeed[0]);
ikarashikota 0:04c98c2cfe16 50 if(timer.read() >= 1){
ikarashikota 6:34f3cf5b05f8 51 encoderAngle[0] += 360;
ikarashikota 0:04c98c2cfe16 52 timer.reset();
ikarashikota 0:04c98c2cfe16 53 }else{
ikarashikota 0:04c98c2cfe16 54 mspeed[0] = 0;
ikarashikota 0:04c98c2cfe16 55 }
ikarashikota 8:1d9530028043 56 if(*button[0] <= 0 && !injecterFlag || *button[0] == NULL){
ikarashikota 0:04c98c2cfe16 57 mspeed[0] = 0;
ikarashikota 0:04c98c2cfe16 58 return;
ikarashikota 0:04c98c2cfe16 59 }
ikarashikota 6:34f3cf5b05f8 60 if(*encoder[0] < encoderAngle[0]){
ikarashikota 0:04c98c2cfe16 61 mspeed[0] = speed;
ikarashikota 3:e62282fc7cf6 62 injecterFlag = true;
ikarashikota 6:34f3cf5b05f8 63 }else{
ikarashikota 0:04c98c2cfe16 64 timer.start();
ikarashikota 0:04c98c2cfe16 65 mspeed[0] = 0;
ikarashikota 3:e62282fc7cf6 66 injecterFlag = false;
ikarashikota 0:04c98c2cfe16 67 }
ikarashikota 0:04c98c2cfe16 68 }
ikarashikota 0:04c98c2cfe16 69
ikarashikota 0:04c98c2cfe16 70 void SEKIkikou::runLoad(float speed)
ikarashikota 0:04c98c2cfe16 71 {
ikarashikota 7:3745c060228e 72 ik[2]->setSpeed(mspeed[1]);
ikarashikota 8:1d9530028043 73 if(*button[1] <= 0 && *button[2] <= 0 || button[1] == NULL || button[2] == NULL){
ikarashikota 0:04c98c2cfe16 74 mspeed[1] = 0;
ikarashikota 0:04c98c2cfe16 75 return;
ikarashikota 0:04c98c2cfe16 76 }else if(*button[1] >= 1){
ikarashikota 0:04c98c2cfe16 77 mspeed[1] = speed;
ikarashikota 0:04c98c2cfe16 78 }else if(*button[2] >= 1){
ikarashikota 0:04c98c2cfe16 79 mspeed[1] = -speed;
ikarashikota 0:04c98c2cfe16 80 }else{
ikarashikota 0:04c98c2cfe16 81 mspeed[1] = 0;
ikarashikota 0:04c98c2cfe16 82 }
ikarashikota 7:3745c060228e 83 }
ikarashikota 7:3745c060228e 84
ikarashikota 10:10c255272359 85 void SEKIkikou::runLift(float speed, int angle)
ikarashikota 7:3745c060228e 86 {
ikarashikota 9:b67cb3335dc7 87 ik[1]->setSpeed(mspeed[2]);
ikarashikota 10:10c255272359 88
ikarashikota 10:10c255272359 89 buff[0] = *button[3] - buff[1];
ikarashikota 10:10c255272359 90 buff[1] = *button[3];
ikarashikota 10:10c255272359 91
ikarashikota 10:10c255272359 92 if(liftFlag && *encoder[2] < envBuff + angle){
ikarashikota 10:10c255272359 93 mspeed[2] = speed;
ikarashikota 10:10c255272359 94 }else{
ikarashikota 10:10c255272359 95 liftFlag = false;
ikarashikota 10:10c255272359 96 mspeed[2] = 0;
ikarashikota 10:10c255272359 97 }
ikarashikota 10:10c255272359 98
ikarashikota 11:9164aa147df1 99 if((buff[0] <= 0 && !liftFlag) || button[3] == NULL){
ikarashikota 7:3745c060228e 100 mspeed[2] = 0;
ikarashikota 7:3745c060228e 101 return;
ikarashikota 10:10c255272359 102 }
ikarashikota 10:10c255272359 103 if(buff[0] >= 1){
ikarashikota 10:10c255272359 104 liftFlag = true;
ikarashikota 10:10c255272359 105 envBuff = *encoder[2];
ikarashikota 7:3745c060228e 106 }
ikarashikota 0:04c98c2cfe16 107 }