NagaokaRoboticsClub_mbedTeam / HOSOKIkikou

HOSOKIkikou class for 2022_NHK_B

Dependents:   2022_NHK_B_UK 2022_NHK_B_sub2

HOSOKIkikou.cpp@3:4f901bdb470f, 2022-10-11 (annotated)

Committer:
ikarashikota
Date:
Tue Oct 11 09:23:21 2022 +0000
Revision:
3:4f901bdb470f
Parent:
2:c50a94f4ff97
Child:
4:c10e0d2844b9
change Load function and Lift function

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