teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

HbUserOpe.cpp@53:b09c062cc31c, 2019-03-04 (annotated)

Committer:
MasashiNomura
Date:
Mon Mar 04 04:03:23 2019 +0000
Revision:
53:b09c062cc31c
Parent:
42:cc8501b824ba
Child:
54:283bb711e0fa
2019/03/04 add engine adjust step

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MasashiNomura 22:24c9c2dedca9 1 #include "HbUserOpe.h"
MasashiNomura 22:24c9c2dedca9 2 #include "fpga.h"
MasashiNomura 22:24c9c2dedca9 3 //=========================================
MasashiNomura 22:24c9c2dedca9 4 //コンストラクタ
MasashiNomura 22:24c9c2dedca9 5 //=========================================
MasashiNomura 22:24c9c2dedca9 6 HbUserOpe::HbUserOpe(){
MasashiNomura 22:24c9c2dedca9 7 oldOpe.w = curOpe.w = 0;
MasashiNomura 36:2cc739c7e4cb 8 AinAxlVal = AinTrtlVal = 0;
MasashiNomura 53:b09c062cc31c 9 for(int i = 0; i < MAX_SW_TYPE; ++i){
MasashiNomura 53:b09c062cc31c 10 counter[i].w = 0;
MasashiNomura 53:b09c062cc31c 11 }
MasashiNomura 22:24c9c2dedca9 12 }
MasashiNomura 22:24c9c2dedca9 13
MasashiNomura 22:24c9c2dedca9 14 //=========================================
MasashiNomura 22:24c9c2dedca9 15 //デストラクタ
MasashiNomura 22:24c9c2dedca9 16 //=========================================
MasashiNomura 22:24c9c2dedca9 17 HbUserOpe::~HbUserOpe(){
MasashiNomura 22:24c9c2dedca9 18 }
MasashiNomura 22:24c9c2dedca9 19
MasashiNomura 22:24c9c2dedca9 20 typUserSw HbUserOpe::GetUserOpe()
MasashiNomura 22:24c9c2dedca9 21 {
MasashiNomura 22:24c9c2dedca9 22 oldOpe = curOpe;
MasashiNomura 22:24c9c2dedca9 23 curOpe.w = fpgaGetUserSw();
MasashiNomura 42:cc8501b824ba 24 if(gf_SwCmd.bf.req){
MasashiNomura 42:cc8501b824ba 25 if(gf_SwCmd.bf.req){
MasashiNomura 42:cc8501b824ba 26 UINT16 mask = 0x1;
MasashiNomura 42:cc8501b824ba 27 mask = mask << gf_SwCmd.bf.val;
MasashiNomura 42:cc8501b824ba 28 curOpe.w |= mask;
MasashiNomura 42:cc8501b824ba 29 gf_SwCmd.bf.req = false;
MasashiNomura 42:cc8501b824ba 30 }
MasashiNomura 42:cc8501b824ba 31 }
MasashiNomura 41:45c982b1c5b6 32 if(gf_Print.d2.bf.sw){
MasashiNomura 42:cc8501b824ba 33 sp.printf("SW : %d%d%d%d%d%d%d%d%d%d ",curOpe.bf.brk_l,curOpe.bf.flt_off,curOpe.bf.r_eng_down,curOpe.bf.r_eng_up,curOpe.bf.rsv_1
MasashiNomura 42:cc8501b824ba 34 ,curOpe.bf.brk_r,curOpe.bf.flt_on,curOpe.bf.f_eng_down,curOpe.bf.f_eng_up, curOpe.bf.all_stop);
MasashiNomura 42:cc8501b824ba 35 //sp.printf("SW : [%04X] ", curOpe.w);
MasashiNomura 41:45c982b1c5b6 36 }
MasashiNomura 22:24c9c2dedca9 37 return curOpe;
MasashiNomura 23:79e20be4bc5b 38 }
MasashiNomura 38:24ee50452755 39 float HbUserOpe::GetAinAccelRaw()
MasashiNomura 38:24ee50452755 40 {
MasashiNomura 38:24ee50452755 41 AinAxlVal = AinAxl.read();
MasashiNomura 38:24ee50452755 42 return AinAxlVal;
MasashiNomura 38:24ee50452755 43 }
MasashiNomura 33:eb260dbfc22a 44 INT16 HbUserOpe::GetAinAccel()
MasashiNomura 33:eb260dbfc22a 45 {
MasashiNomura 34:234b87f3e6ce 46 // max rpm 8500 ユーザー使用分として7000をマックスとする(仮)
MasashiNomura 34:234b87f3e6ce 47 // 0 ~ 1.00 -> 0.01 ~ 0.97を使用する（遊び分として）
MasashiNomura 33:eb260dbfc22a 48 INT16 ret = 0;
MasashiNomura 34:234b87f3e6ce 49 AinAxlVal = AinAxl.read();
MasashiNomura 34:234b87f3e6ce 50 if(MIN_EFF_ANA_VAL > AinAxlVal){
MasashiNomura 33:eb260dbfc22a 51 ret = 0;
MasashiNomura 34:234b87f3e6ce 52 } else if(AinAxlVal >= MIN_EFF_ANA_VAL && AinAxlVal <= MAX_EFF_ANA_VAL){
MasashiNomura 34:234b87f3e6ce 53 ret = (MAX_RPM_USERSET - MIN_RPM_TOTAL) * (AinAxlVal - MIN_EFF_ANA_VAL) / (MAX_EFF_ANA_VAL - MIN_EFF_ANA_VAL) + MIN_RPM_TOTAL;
MasashiNomura 34:234b87f3e6ce 54 } else if(AinAxlVal > MAX_EFF_ANA_VAL){
MasashiNomura 34:234b87f3e6ce 55 ret = MAX_RPM_USERSET;
MasashiNomura 33:eb260dbfc22a 56 }else{
MasashiNomura 33:eb260dbfc22a 57 ret = 0;
MasashiNomura 33:eb260dbfc22a 58 }
MasashiNomura 41:45c982b1c5b6 59 if(gf_Print.d2.bf.ain == true){
MasashiNomura 41:45c982b1c5b6 60 sp.printf("Axl Rpm=%d val=%f ",ret,AinAxlVal);
MasashiNomura 33:eb260dbfc22a 61 }
MasashiNomura 33:eb260dbfc22a 62 //sp.printf("Axl Rpm=%d val=%f\r\n",ret,anaval);
MasashiNomura 33:eb260dbfc22a 63 return ret;
MasashiNomura 33:eb260dbfc22a 64 }
MasashiNomura 34:234b87f3e6ce 65
MasashiNomura 38:24ee50452755 66 float HbUserOpe::GetAinThrottleRaw()
MasashiNomura 38:24ee50452755 67 {
MasashiNomura 38:24ee50452755 68 AinTrtlVal = AinAxl.read();
MasashiNomura 38:24ee50452755 69 return AinTrtlVal;
MasashiNomura 38:24ee50452755 70 }
MasashiNomura 38:24ee50452755 71
MasashiNomura 36:2cc739c7e4cb 72 INT16 HbUserOpe::GetAinThrottle()
MasashiNomura 36:2cc739c7e4cb 73 {
MasashiNomura 36:2cc739c7e4cb 74 INT16 ret = 0;
MasashiNomura 38:24ee50452755 75 AinTrtlVal = AinAxl.read();
MasashiNomura 38:24ee50452755 76 //AinTrtlVal = AinThrottle.read();
MasashiNomura 38:24ee50452755 77 ret = (INT16)(MAX_VAL_12BIT * AinTrtlVal);
MasashiNomura 41:45c982b1c5b6 78 if(gf_Print.d2.bf.ain == true){
MasashiNomura 41:45c982b1c5b6 79 sp.printf("Trottle Val=%d analog val=%f ",ret,AinTrtlVal);
MasashiNomura 36:2cc739c7e4cb 80 }
MasashiNomura 36:2cc739c7e4cb 81 return ret;
MasashiNomura 36:2cc739c7e4cb 82 }
MasashiNomura 36:2cc739c7e4cb 83
MasashiNomura 23:79e20be4bc5b 84 bool HbUserOpe::ChkCtrlSW(SW_TYPE styp)
MasashiNomura 23:79e20be4bc5b 85 {
MasashiNomura 36:2cc739c7e4cb 86 if(styp == BRK_L) {return curOpe.bf.brk_l;}
MasashiNomura 36:2cc739c7e4cb 87 else if(styp == BRK_R) {return curOpe.bf.brk_r;}
MasashiNomura 36:2cc739c7e4cb 88 else if(styp == FLT_ON) {return curOpe.bf.flt_on;}
MasashiNomura 34:234b87f3e6ce 89 else if(styp == FLT_OFF) {return curOpe.bf.flt_off;}
MasashiNomura 36:2cc739c7e4cb 90 else if(styp == F_ENG_UP) {return curOpe.bf.f_eng_up;}
MasashiNomura 36:2cc739c7e4cb 91 else if(styp == F_ENG_DOWN) {return curOpe.bf.f_eng_down;}
MasashiNomura 36:2cc739c7e4cb 92 else if(styp == R_ENG_UP) {return curOpe.bf.r_eng_up;}
MasashiNomura 36:2cc739c7e4cb 93 else if(styp == R_ENG_DOWN) {return curOpe.bf.r_eng_down;}
MasashiNomura 36:2cc739c7e4cb 94 else if(styp == R_1) {return curOpe.bf.rsv_1;}
MasashiNomura 36:2cc739c7e4cb 95 else if(styp == ALL_STOP) {return curOpe.bf.all_stop;}
MasashiNomura 31:56c554c560c1 96 else {return false;}
MasashiNomura 26:732bc37fbefd 97 }
MasashiNomura 23:79e20be4bc5b 98 bool HbUserOpe::ChkCtrlSwAny()
MasashiNomura 23:79e20be4bc5b 99 {
MasashiNomura 23:79e20be4bc5b 100 UINT16 mask = 0x1;
MasashiNomura 34:234b87f3e6ce 101 for(int i = 0; i < 10; ++i){
MasashiNomura 23:79e20be4bc5b 102 if(curOpe.w & mask){
MasashiNomura 23:79e20be4bc5b 103 return true;
MasashiNomura 23:79e20be4bc5b 104 }
MasashiNomura 23:79e20be4bc5b 105 mask = mask << 1;
MasashiNomura 23:79e20be4bc5b 106 }
MasashiNomura 23:79e20be4bc5b 107 return false;
MasashiNomura 23:79e20be4bc5b 108 }
MasashiNomura 36:2cc739c7e4cb 109 bool HbUserOpe::ChkCtrlSwBoth(SW_TYPE styp1,SW_TYPE styp2)
MasashiNomura 36:2cc739c7e4cb 110 {
MasashiNomura 36:2cc739c7e4cb 111 bool sw1,sw2;
MasashiNomura 36:2cc739c7e4cb 112 sw1 = sw2 = false;
MasashiNomura 36:2cc739c7e4cb 113 if(styp1 == BRK_L) {sw1 = curOpe.bf.brk_l;}
MasashiNomura 36:2cc739c7e4cb 114 else if(styp1 == BRK_R) {sw1 = curOpe.bf.brk_r;}
MasashiNomura 36:2cc739c7e4cb 115 else if(styp1 == FLT_ON) {sw1 = curOpe.bf.flt_on;}
MasashiNomura 36:2cc739c7e4cb 116 else if(styp1 == FLT_OFF) {sw1 = curOpe.bf.flt_off;}
MasashiNomura 36:2cc739c7e4cb 117 else if(styp1 == F_ENG_UP) {sw1 = curOpe.bf.f_eng_up;}
MasashiNomura 36:2cc739c7e4cb 118 else if(styp1 == F_ENG_DOWN) {sw1 = curOpe.bf.f_eng_down;}
MasashiNomura 36:2cc739c7e4cb 119 else if(styp1 == R_ENG_UP) {sw1 = curOpe.bf.r_eng_up;}
MasashiNomura 36:2cc739c7e4cb 120 else if(styp1 == R_ENG_DOWN) {sw1 = curOpe.bf.r_eng_down;}
MasashiNomura 36:2cc739c7e4cb 121 else if(styp1 == R_1) {sw1 = curOpe.bf.rsv_1;}
MasashiNomura 36:2cc739c7e4cb 122 else if(styp1 == ALL_STOP) {sw1 = curOpe.bf.all_stop;}
MasashiNomura 36:2cc739c7e4cb 123 else {sw1 = false;}
MasashiNomura 36:2cc739c7e4cb 124 if(styp2 == BRK_L) {sw2 = curOpe.bf.brk_l;}
MasashiNomura 36:2cc739c7e4cb 125 else if(styp2 == BRK_R) {sw2 = curOpe.bf.brk_r;}
MasashiNomura 36:2cc739c7e4cb 126 else if(styp2 == FLT_ON) {sw2 = curOpe.bf.flt_on;}
MasashiNomura 36:2cc739c7e4cb 127 else if(styp2 == FLT_OFF) {sw2 = curOpe.bf.flt_off;}
MasashiNomura 36:2cc739c7e4cb 128 else if(styp2 == F_ENG_UP) {sw2 = curOpe.bf.f_eng_up;}
MasashiNomura 36:2cc739c7e4cb 129 else if(styp2 == F_ENG_DOWN) {sw2 = curOpe.bf.f_eng_down;}
MasashiNomura 36:2cc739c7e4cb 130 else if(styp2 == R_ENG_UP) {sw2 = curOpe.bf.r_eng_up;}
MasashiNomura 36:2cc739c7e4cb 131 else if(styp2 == R_ENG_DOWN) {sw2 = curOpe.bf.r_eng_down;}
MasashiNomura 36:2cc739c7e4cb 132 else if(styp2 == R_1) {sw2 = curOpe.bf.rsv_1;}
MasashiNomura 36:2cc739c7e4cb 133 else if(styp2 == ALL_STOP) {sw2 = curOpe.bf.all_stop;}
MasashiNomura 36:2cc739c7e4cb 134 else {sw2 = false;}
MasashiNomura 36:2cc739c7e4cb 135
MasashiNomura 36:2cc739c7e4cb 136 return sw1 && sw2;
MasashiNomura 36:2cc739c7e4cb 137 }
MasashiNomura 36:2cc739c7e4cb 138
MasashiNomura 30:13ada1a24c59 139 bool HbUserOpe::ChkCtrlSwRiseEdge(SW_TYPE styp)
MasashiNomura 26:732bc37fbefd 140 {
MasashiNomura 53:b09c062cc31c 141 if(counter[styp].bf.flg){
MasashiNomura 53:b09c062cc31c 142 ++counter[styp].bf.cnt;
MasashiNomura 53:b09c062cc31c 143 if(counter[styp].bf.cnt > CNT_NUM_RE){
MasashiNomura 53:b09c062cc31c 144 counter[styp].bf.cnt = 0;
MasashiNomura 53:b09c062cc31c 145 counter[styp].bf.flg = false;
MasashiNomura 53:b09c062cc31c 146 }
MasashiNomura 53:b09c062cc31c 147 return false;
MasashiNomura 53:b09c062cc31c 148 } else{
MasashiNomura 53:b09c062cc31c 149 if(styp == BRK_L) return counter[BRK_L].bf.flg = curOpe.bf.brk_l && !oldOpe.bf.brk_l;
MasashiNomura 53:b09c062cc31c 150 if(styp == BRK_R) return counter[BRK_R].bf.flg = curOpe.bf.brk_r && !oldOpe.bf.brk_r;
MasashiNomura 53:b09c062cc31c 151 if(styp == FLT_ON) return counter[FLT_ON].bf.flg = curOpe.bf.flt_on && !oldOpe.bf.flt_on;
MasashiNomura 53:b09c062cc31c 152 if(styp == FLT_OFF) return counter[FLT_OFF].bf.flg = curOpe.bf.flt_off && !oldOpe.bf.flt_off;
MasashiNomura 53:b09c062cc31c 153 if(styp == F_ENG_UP) return counter[F_ENG_UP].bf.flg = curOpe.bf.f_eng_up && !oldOpe.bf.f_eng_up;
MasashiNomura 53:b09c062cc31c 154 if(styp == F_ENG_DOWN) return counter[F_ENG_DOWN].bf.flg = curOpe.bf.f_eng_down && !oldOpe.bf.f_eng_down;
MasashiNomura 53:b09c062cc31c 155 if(styp == R_ENG_UP) return counter[R_ENG_UP].bf.flg = curOpe.bf.r_eng_up && !oldOpe.bf.r_eng_up;
MasashiNomura 53:b09c062cc31c 156 if(styp == R_ENG_DOWN) return counter[R_ENG_DOWN].bf.flg = curOpe.bf.r_eng_down && !oldOpe.bf.r_eng_down;
MasashiNomura 53:b09c062cc31c 157 if(styp == R_1) return counter[R_1].bf.flg = curOpe.bf.rsv_1 && !oldOpe.bf.rsv_1;
MasashiNomura 53:b09c062cc31c 158 if(styp == ALL_STOP) return counter[ALL_STOP].bf.flg = curOpe.bf.all_stop && !oldOpe.bf.all_stop;
MasashiNomura 53:b09c062cc31c 159 return false;
MasashiNomura 53:b09c062cc31c 160 }
MasashiNomura 53:b09c062cc31c 161
MasashiNomura 53:b09c062cc31c 162 // if(styp == BRK_L)
MasashiNomura 53:b09c062cc31c 163 // {
MasashiNomura 53:b09c062cc31c 164 // return curOpe.bf.brk_l && !oldOpe.bf.brk_l;
MasashiNomura 53:b09c062cc31c 165 // }
MasashiNomura 53:b09c062cc31c 166 // else if(styp == BRK_R)
MasashiNomura 53:b09c062cc31c 167 // {
MasashiNomura 53:b09c062cc31c 168 // return curOpe.bf.brk_r && !oldOpe.bf.brk_r;
MasashiNomura 53:b09c062cc31c 169 // }
MasashiNomura 53:b09c062cc31c 170 // else if(styp == FLT_ON)
MasashiNomura 53:b09c062cc31c 171 // {
MasashiNomura 53:b09c062cc31c 172 // return curOpe.bf.flt_on && !oldOpe.bf.flt_on;
MasashiNomura 53:b09c062cc31c 173 // }
MasashiNomura 53:b09c062cc31c 174 // else if(styp == FLT_OFF)
MasashiNomura 53:b09c062cc31c 175 // {
MasashiNomura 53:b09c062cc31c 176 // return curOpe.bf.flt_off && !oldOpe.bf.flt_off;
MasashiNomura 53:b09c062cc31c 177 // }
MasashiNomura 53:b09c062cc31c 178 // else if(styp == F_ENG_UP)
MasashiNomura 53:b09c062cc31c 179 // {
MasashiNomura 53:b09c062cc31c 180 // return curOpe.bf.f_eng_up && !oldOpe.bf.f_eng_up;
MasashiNomura 53:b09c062cc31c 181 // }
MasashiNomura 53:b09c062cc31c 182 // else if(styp == F_ENG_DOWN)
MasashiNomura 53:b09c062cc31c 183 // {
MasashiNomura 53:b09c062cc31c 184 // return curOpe.bf.f_eng_down && !oldOpe.bf.f_eng_down;
MasashiNomura 53:b09c062cc31c 185 // }
MasashiNomura 53:b09c062cc31c 186 // else if(styp == R_ENG_UP)
MasashiNomura 53:b09c062cc31c 187 // {
MasashiNomura 53:b09c062cc31c 188 // return curOpe.bf.r_eng_up && !oldOpe.bf.r_eng_up;
MasashiNomura 53:b09c062cc31c 189 // }
MasashiNomura 53:b09c062cc31c 190 // else if(styp == R_ENG_DOWN)
MasashiNomura 53:b09c062cc31c 191 // {
MasashiNomura 53:b09c062cc31c 192 // return curOpe.bf.r_eng_down && !oldOpe.bf.r_eng_down;
MasashiNomura 53:b09c062cc31c 193 // }
MasashiNomura 53:b09c062cc31c 194 // else if(styp == R_1)
MasashiNomura 53:b09c062cc31c 195 // {
MasashiNomura 53:b09c062cc31c 196 // return curOpe.bf.rsv_1 && !oldOpe.bf.rsv_1;
MasashiNomura 53:b09c062cc31c 197 // }
MasashiNomura 53:b09c062cc31c 198 // else if(styp == ALL_STOP)
MasashiNomura 53:b09c062cc31c 199 // {
MasashiNomura 53:b09c062cc31c 200 // return curOpe.bf.all_stop && !oldOpe.bf.all_stop;
MasashiNomura 53:b09c062cc31c 201 // }
MasashiNomura 53:b09c062cc31c 202 // else
MasashiNomura 53:b09c062cc31c 203 // {
MasashiNomura 53:b09c062cc31c 204 // return false;
MasashiNomura 53:b09c062cc31c 205 // }
MasashiNomura 26:732bc37fbefd 206 }