NagaokaRoboticsClub_mbedTeam / HOSOKIkikou

HOSOKIkikou class for 2022_NHK_B

Dependents:   2022_NHK_B_UK 2022_NHK_B_sub2

HOSOKIkikou.cpp@1:5cbd36d7e30a, 2022-10-10 (annotated)

Committer:
ikarashikota
Date:
Mon Oct 10 09:54:58 2022 +0000
Revision:
1:5cbd36d7e30a
Parent:
0:2e7a4f14e9f3
Child:
2:c50a94f4ff97
fixed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikarashikota 0:2e7a4f14e9f3 1
ikarashikota 0:2e7a4f14e9f3 2
ikarashikota 0:2e7a4f14e9f3 3 #include "HOSOKIkikou.h"
ikarashikota 0:2e7a4f14e9f3 4
ikarashikota 0:2e7a4f14e9f3 5 HOSOKIkikou::HOSOKIkikou(ikarashiMDC *ik1, ikarashiMDC *ik2, ikarashiMDC *ik3, ikarashiMDC *ik4, uint8_t *b1, uint8_t *b2, uint8_t *b3, uint8_t *b4, int *env1, int *env2, int *env3){
ikarashikota 0:2e7a4f14e9f3 6 init(ik1, ik2, ik3, ik4, b1, b2, b3, b4, env1, env2, env3);
ikarashikota 0:2e7a4f14e9f3 7 }
ikarashikota 0:2e7a4f14e9f3 8
ikarashikota 0:2e7a4f14e9f3 9 void HOSOKIkikou::init(ikarashiMDC *ik1, ikarashiMDC *ik2, ikarashiMDC *ik3, ikarashiMDC *ik4, uint8_t *b1, uint8_t *b2, uint8_t *b3, uint8_t *b4, int *env1, int *env2, int *env3){
ikarashikota 0:2e7a4f14e9f3 10 ik[0] = ik1; //射出右
ikarashikota 0:2e7a4f14e9f3 11 ik[1] = ik2; //射出左
ikarashikota 0:2e7a4f14e9f3 12 ik[2] = ik3; //将校
ikarashikota 0:2e7a4f14e9f3 13 ik[3] = ik4; //装填
ikarashikota 0:2e7a4f14e9f3 14 button[0] = b1; //射出
ikarashikota 0:2e7a4f14e9f3 15 button[1] = b2; //上
ikarashikota 0:2e7a4f14e9f3 16 button[2] = b3; //下
ikarashikota 0:2e7a4f14e9f3 17 button[3] = b4; //装填
ikarashikota 0:2e7a4f14e9f3 18 encoder[0] = env1; //射出
ikarashikota 0:2e7a4f14e9f3 19 encoder[1] = env2; //将校
ikarashikota 0:2e7a4f14e9f3 20 encoder[2] = env3; //装填
ikarashikota 1:5cbd36d7e30a 21 for(int i=0; i<4; i++){
ikarashikota 1:5cbd36d7e30a 22 mspeed[i] = 0;
ikarashikota 1:5cbd36d7e30a 23 if(i < 2) buff[i] = 0;
ikarashikota 1:5cbd36d7e30a 24 }
ikarashikota 0:2e7a4f14e9f3 25 injecterFlag = false;
ikarashikota 0:2e7a4f14e9f3 26 }
ikarashikota 0:2e7a4f14e9f3 27
ikarashikota 0:2e7a4f14e9f3 28 void HOSOKIkikou::runAll(float speedIRight, float speedILeft, float speedLoad, float speedLift)
ikarashikota 0:2e7a4f14e9f3 29 {
ikarashikota 0:2e7a4f14e9f3 30 runInjection(speedIRight, speedILeft);
ikarashikota 0:2e7a4f14e9f3 31 runLoad(speedLoad);
ikarashikota 0:2e7a4f14e9f3 32 runLift(speedLift);
ikarashikota 0:2e7a4f14e9f3 33 }
ikarashikota 0:2e7a4f14e9f3 34
ikarashikota 0:2e7a4f14e9f3 35 void HOSOKIkikou::stopAll(){
ikarashikota 0:2e7a4f14e9f3 36 //init(NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
ikarashikota 0:2e7a4f14e9f3 37 for(int i=0; i<4; i++){
ikarashikota 0:2e7a4f14e9f3 38 button[i] = NULL;
ikarashikota 0:2e7a4f14e9f3 39 mspeed[i] = 0;
ikarashikota 0:2e7a4f14e9f3 40 if(i < 3){
ikarashikota 0:2e7a4f14e9f3 41 encoder[i] = NULL;
ikarashikota 0:2e7a4f14e9f3 42 }
ikarashikota 0:2e7a4f14e9f3 43 }
ikarashikota 0:2e7a4f14e9f3 44 injecterFlag = false;
ikarashikota 0:2e7a4f14e9f3 45 }
ikarashikota 0:2e7a4f14e9f3 46
ikarashikota 0:2e7a4f14e9f3 47 void HOSOKIkikou::runInjection(float speedRight, float speedLeft)
ikarashikota 0:2e7a4f14e9f3 48 {
ikarashikota 0:2e7a4f14e9f3 49 ik[0]->setSpeed(mspeed[0]);
ikarashikota 0:2e7a4f14e9f3 50 ik[1]->setSpeed(mspeed[1]);
ikarashikota 0:2e7a4f14e9f3 51 if(injecterFlag){
ikarashikota 0:2e7a4f14e9f3 52 mspeed[0] = speedRight;
ikarashikota 0:2e7a4f14e9f3 53 mspeed[1] = speedLeft;
ikarashikota 0:2e7a4f14e9f3 54 }else{
ikarashikota 0:2e7a4f14e9f3 55 mspeed[0] = 0;
ikarashikota 0:2e7a4f14e9f3 56 mspeed[1] = 0;
ikarashikota 0:2e7a4f14e9f3 57 }
ikarashikota 1:5cbd36d7e30a 58
ikarashikota 1:5cbd36d7e30a 59 buff[0] = *button[0] - buff[1];
ikarashikota 1:5cbd36d7e30a 60 buff[1] = *button[0];
ikarashikota 1:5cbd36d7e30a 61
ikarashikota 1:5cbd36d7e30a 62 if(buff[0] <= 0){
ikarashikota 1:5cbd36d7e30a 63 return;
ikarashikota 1:5cbd36d7e30a 64 }else{
ikarashikota 1:5cbd36d7e30a 65 injecterFlag = !injecterFlag;
ikarashikota 1:5cbd36d7e30a 66 }
ikarashikota 0:2e7a4f14e9f3 67 }
ikarashikota 0:2e7a4f14e9f3 68
ikarashikota 0:2e7a4f14e9f3 69 void HOSOKIkikou::runLoad(float speed)
ikarashikota 0:2e7a4f14e9f3 70 {
ikarashikota 0:2e7a4f14e9f3 71 ik[3]->setSpeed(mspeed[3]);
ikarashikota 0:2e7a4f14e9f3 72 if(*button[3] <= 0){
ikarashikota 0:2e7a4f14e9f3 73 mspeed[3] = 0;
ikarashikota 0:2e7a4f14e9f3 74 return;
ikarashikota 0:2e7a4f14e9f3 75 }else if(*button[3] >= 1){
ikarashikota 0:2e7a4f14e9f3 76 mspeed[3] = speed;
ikarashikota 0:2e7a4f14e9f3 77 }else{
ikarashikota 0:2e7a4f14e9f3 78 mspeed[3] = 0;
ikarashikota 0:2e7a4f14e9f3 79 }
ikarashikota 0:2e7a4f14e9f3 80 }
ikarashikota 0:2e7a4f14e9f3 81
ikarashikota 0:2e7a4f14e9f3 82 void HOSOKIkikou::runLift(float speed)
ikarashikota 0:2e7a4f14e9f3 83 {
ikarashikota 0:2e7a4f14e9f3 84 ik[2]->setSpeed(mspeed[2]);
ikarashikota 0:2e7a4f14e9f3 85 if(*button[1] <= 0 && *button[2] <= 0){
ikarashikota 0:2e7a4f14e9f3 86 mspeed[2] = 0;
ikarashikota 0:2e7a4f14e9f3 87 return;
ikarashikota 0:2e7a4f14e9f3 88 }else if(*button[1] >= 1){
ikarashikota 0:2e7a4f14e9f3 89 mspeed[2] = speed;
ikarashikota 0:2e7a4f14e9f3 90 }else if(*button[2] >= 1){
ikarashikota 0:2e7a4f14e9f3 91 mspeed[2] = -speed;
ikarashikota 0:2e7a4f14e9f3 92 }else{
ikarashikota 0:2e7a4f14e9f3 93 mspeed[2] = 0;
ikarashikota 0:2e7a4f14e9f3 94 }
ikarashikota 0:2e7a4f14e9f3 95 }