NagaokaRoboticsClub_mbedTeam / HOSOKIkikou

HOSOKIkikou class for 2022_NHK_B

Dependents:   2022_NHK_B_UK 2022_NHK_B_sub2

HOSOKIkikou.cpp@8:d73c83c92932, 23 months ago (annotated)

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