z ysaito / Mbed 2 deprecated APro_B2DTst_1_ResAnaCtrl_LPC824

https://os.mbed.com/users/sayzyas

Dependencies:   QEI TextLCD mbed

mcchk.cpp@0:73dd48be5ca6, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:15 2018 +0000
Revision:
0:73dd48be5ca6
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:73dd48be5ca6 1 #include "mbed.h"
sayzyas 0:73dd48be5ca6 2 #include "common.h"
sayzyas 0:73dd48be5ca6 3 #include <math.h>
sayzyas 0:73dd48be5ca6 4 #include "mcchk.h"
sayzyas 0:73dd48be5ca6 5
sayzyas 0:73dd48be5ca6 6
sayzyas 0:73dd48be5ca6 7 // Motor current
sayzyas 0:73dd48be5ca6 8 AnalogIn mctr3_m2c(A5); // LB crawler
sayzyas 0:73dd48be5ca6 9 AnalogIn mctr3_m1c(A4); // RF crawler
sayzyas 0:73dd48be5ca6 10 AnalogIn mctr2_m2c(A3); // LB transform
sayzyas 0:73dd48be5ca6 11 AnalogIn mctr2_m1c(A2); // RF Transform
sayzyas 0:73dd48be5ca6 12 AnalogIn mctr1_m2c(A1); // Tilt
sayzyas 0:73dd48be5ca6 13 AnalogIn mctr1_m1c(A0); // B1: Pan, B2:Winch
sayzyas 0:73dd48be5ca6 14
sayzyas 0:73dd48be5ca6 15
sayzyas 0:73dd48be5ca6 16 mcchk::mcchk()
sayzyas 0:73dd48be5ca6 17 {
sayzyas 0:73dd48be5ca6 18 cnt_mclock_LBCRW_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 19 cnt_mclock_LBCRW_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 20 cnt_mclock_RFCRW_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 21 cnt_mclock_RFCRW_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 22 cnt_mclock_LBTFM_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 23 cnt_mclock_LBTFM_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 24 cnt_mclock_RFTFM_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 25 cnt_mclock_RFTFM_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 26 cnt_mclock_CMPAN_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 27 cnt_mclock_CMPAN_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 28 cnt_mclock_CTILT_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 29 cnt_mclock_CTILT_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 30 cnt_mclock_WINCH_f = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 31 cnt_mclock_WINCH_r = 0; // motor lock counter
sayzyas 0:73dd48be5ca6 32 }
sayzyas 0:73dd48be5ca6 33
sayzyas 0:73dd48be5ca6 34
sayzyas 0:73dd48be5ca6 35 // --------------------------------------------------------------
sayzyas 0:73dd48be5ca6 36 // Motor Current Calibration Function
sayzyas 0:73dd48be5ca6 37 // Arg: Motor Number 1 - 3
sayzyas 0:73dd48be5ca6 38 // --------------------------------------------------------------
sayzyas 0:73dd48be5ca6 39 void mcchk::set_init_mc( int cnt )
sayzyas 0:73dd48be5ca6 40 {
sayzyas 0:73dd48be5ca6 41
sayzyas 0:73dd48be5ca6 42 m_LBCRW_center_value = 0;
sayzyas 0:73dd48be5ca6 43 m_RFCRW_center_value = 0;
sayzyas 0:73dd48be5ca6 44 m_LBTFM_center_value = 0;
sayzyas 0:73dd48be5ca6 45 m_RFTFM_center_value = 0;
sayzyas 0:73dd48be5ca6 46 m_WINCH_center_value = 0;
sayzyas 0:73dd48be5ca6 47 m_CMPAN_center_value = 0;
sayzyas 0:73dd48be5ca6 48 m_CTILT_center_value = 0;
sayzyas 0:73dd48be5ca6 49
sayzyas 0:73dd48be5ca6 50 for( int i = 0; i < cnt; i++ )
sayzyas 0:73dd48be5ca6 51 {
sayzyas 0:73dd48be5ca6 52 m_LBCRW_center_value += mctr3_m1c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 53 m_RFCRW_center_value += mctr3_m2c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 54 m_LBTFM_center_value += mctr2_m1c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 55 m_RFTFM_center_value += mctr2_m2c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 56 m_WINCH_center_value += mctr1_m1c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 57 m_CMPAN_center_value += mctr1_m1c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 58 m_CTILT_center_value += mctr1_m2c.read() * 100.0f;
sayzyas 0:73dd48be5ca6 59 }
sayzyas 0:73dd48be5ca6 60
sayzyas 0:73dd48be5ca6 61 m_LBCRW_center_value /= cnt;
sayzyas 0:73dd48be5ca6 62 m_RFCRW_center_value /= cnt;
sayzyas 0:73dd48be5ca6 63 m_LBTFM_center_value /= cnt;
sayzyas 0:73dd48be5ca6 64 m_RFTFM_center_value /= cnt;
sayzyas 0:73dd48be5ca6 65 m_WINCH_center_value /= cnt;
sayzyas 0:73dd48be5ca6 66 m_CMPAN_center_value /= cnt;
sayzyas 0:73dd48be5ca6 67 m_CTILT_center_value /= cnt;
sayzyas 0:73dd48be5ca6 68 }
sayzyas 0:73dd48be5ca6 69
sayzyas 0:73dd48be5ca6 70
sayzyas 0:73dd48be5ca6 71 float mcchk::rd_motorCurrent( int no )
sayzyas 0:73dd48be5ca6 72 {
sayzyas 0:73dd48be5ca6 73 if( no == 11 ) return mctr1_m1c.read()*100.0f;
sayzyas 0:73dd48be5ca6 74 else if( no == 12 ) return mctr1_m2c.read()*100.0f;
sayzyas 0:73dd48be5ca6 75 else if( no == 21 ) return mctr2_m1c.read()*100.0f;
sayzyas 0:73dd48be5ca6 76 else if( no == 22 ) return mctr2_m2c.read()*100.0f;
sayzyas 0:73dd48be5ca6 77 else if( no == 31 ) return mctr3_m1c.read()*100.0f;
sayzyas 0:73dd48be5ca6 78 else if( no == 32 ) return mctr3_m2c.read()*100.0f;
sayzyas 0:73dd48be5ca6 79 return 0;
sayzyas 0:73dd48be5ca6 80 }
sayzyas 0:73dd48be5ca6 81
sayzyas 0:73dd48be5ca6 82
sayzyas 0:73dd48be5ca6 83 bool mcchk::rdnchk_motorCurrent(
sayzyas 0:73dd48be5ca6 84 int8_t motor_number,
sayzyas 0:73dd48be5ca6 85 int8_t motor_dir,
sayzyas 0:73dd48be5ca6 86 int8_t motor_lock_chk_cnt
sayzyas 0:73dd48be5ca6 87 ){
sayzyas 0:73dd48be5ca6 88 bool mcchk = false;
sayzyas 0:73dd48be5ca6 89 bool rts = false;
sayzyas 0:73dd48be5ca6 90
sayzyas 0:73dd48be5ca6 91 // Check RF-Crawler
sayzyas 0:73dd48be5ca6 92 if( motor_number == MOTOR_RFCRW )
sayzyas 0:73dd48be5ca6 93 {
sayzyas 0:73dd48be5ca6 94 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 95 {
sayzyas 0:73dd48be5ca6 96 mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_f);
sayzyas 0:73dd48be5ca6 97 if( mcchk == true )
sayzyas 0:73dd48be5ca6 98 {
sayzyas 0:73dd48be5ca6 99 cnt_mclock_RFCRW_f += 1;
sayzyas 0:73dd48be5ca6 100 }
sayzyas 0:73dd48be5ca6 101 else
sayzyas 0:73dd48be5ca6 102 { if( cnt_mclock_RFCRW_f > 0 )
sayzyas 0:73dd48be5ca6 103 {
sayzyas 0:73dd48be5ca6 104 cnt_mclock_RFCRW_f -= 1;
sayzyas 0:73dd48be5ca6 105 }
sayzyas 0:73dd48be5ca6 106 else
sayzyas 0:73dd48be5ca6 107 {
sayzyas 0:73dd48be5ca6 108 cnt_mclock_RFCRW_f = 0;
sayzyas 0:73dd48be5ca6 109 }
sayzyas 0:73dd48be5ca6 110 }
sayzyas 0:73dd48be5ca6 111 if( cnt_mclock_RFCRW_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 112 {
sayzyas 0:73dd48be5ca6 113 flg_mclock_RFCRW = 1;
sayzyas 0:73dd48be5ca6 114 }
sayzyas 0:73dd48be5ca6 115 else
sayzyas 0:73dd48be5ca6 116 {
sayzyas 0:73dd48be5ca6 117 flg_mclock_RFCRW = 0;
sayzyas 0:73dd48be5ca6 118 rts = false; // NO LOCK
sayzyas 0:73dd48be5ca6 119 }
sayzyas 0:73dd48be5ca6 120 }
sayzyas 0:73dd48be5ca6 121 else
sayzyas 0:73dd48be5ca6 122 {
sayzyas 0:73dd48be5ca6 123 mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_r);
sayzyas 0:73dd48be5ca6 124 if( mcchk == true )
sayzyas 0:73dd48be5ca6 125 {
sayzyas 0:73dd48be5ca6 126 cnt_mclock_RFCRW_r += 1;
sayzyas 0:73dd48be5ca6 127 }
sayzyas 0:73dd48be5ca6 128 else
sayzyas 0:73dd48be5ca6 129 { if( cnt_mclock_RFCRW_r > 0 )
sayzyas 0:73dd48be5ca6 130 {
sayzyas 0:73dd48be5ca6 131 cnt_mclock_RFCRW_r -= 1;
sayzyas 0:73dd48be5ca6 132 }
sayzyas 0:73dd48be5ca6 133 else
sayzyas 0:73dd48be5ca6 134 {
sayzyas 0:73dd48be5ca6 135 cnt_mclock_RFCRW_r = 0;
sayzyas 0:73dd48be5ca6 136 }
sayzyas 0:73dd48be5ca6 137 }
sayzyas 0:73dd48be5ca6 138 if( cnt_mclock_RFCRW_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 139 {
sayzyas 0:73dd48be5ca6 140 flg_mclock_RFCRW = 1;
sayzyas 0:73dd48be5ca6 141 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 142 }
sayzyas 0:73dd48be5ca6 143 else
sayzyas 0:73dd48be5ca6 144 {
sayzyas 0:73dd48be5ca6 145 flg_mclock_RFCRW = 0;
sayzyas 0:73dd48be5ca6 146 rts = false; // NO LOCK
sayzyas 0:73dd48be5ca6 147 }
sayzyas 0:73dd48be5ca6 148 }
sayzyas 0:73dd48be5ca6 149 }
sayzyas 0:73dd48be5ca6 150 // Check LB-Crawler
sayzyas 0:73dd48be5ca6 151 else if( motor_number == MOTOR_LBCRW )
sayzyas 0:73dd48be5ca6 152 {
sayzyas 0:73dd48be5ca6 153 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 154 {
sayzyas 0:73dd48be5ca6 155 mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_f);
sayzyas 0:73dd48be5ca6 156 if( mcchk == true )
sayzyas 0:73dd48be5ca6 157 {
sayzyas 0:73dd48be5ca6 158 cnt_mclock_LBCRW_f += 1;
sayzyas 0:73dd48be5ca6 159 }
sayzyas 0:73dd48be5ca6 160 else
sayzyas 0:73dd48be5ca6 161 { if( cnt_mclock_LBCRW_f > 0 )
sayzyas 0:73dd48be5ca6 162 {
sayzyas 0:73dd48be5ca6 163 cnt_mclock_LBCRW_f -= 1;
sayzyas 0:73dd48be5ca6 164 }
sayzyas 0:73dd48be5ca6 165 else
sayzyas 0:73dd48be5ca6 166 {
sayzyas 0:73dd48be5ca6 167 cnt_mclock_LBCRW_f = 0;
sayzyas 0:73dd48be5ca6 168 }
sayzyas 0:73dd48be5ca6 169 }
sayzyas 0:73dd48be5ca6 170 if( cnt_mclock_LBCRW_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 171 {
sayzyas 0:73dd48be5ca6 172 flg_mclock_LBCRW = 1;
sayzyas 0:73dd48be5ca6 173 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 174 }
sayzyas 0:73dd48be5ca6 175 else
sayzyas 0:73dd48be5ca6 176 {
sayzyas 0:73dd48be5ca6 177 flg_mclock_LBCRW = 0;
sayzyas 0:73dd48be5ca6 178 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 179 }
sayzyas 0:73dd48be5ca6 180 }
sayzyas 0:73dd48be5ca6 181 else
sayzyas 0:73dd48be5ca6 182 {
sayzyas 0:73dd48be5ca6 183 mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_r);
sayzyas 0:73dd48be5ca6 184 if( mcchk == true )
sayzyas 0:73dd48be5ca6 185 {
sayzyas 0:73dd48be5ca6 186 cnt_mclock_LBCRW_r += 1;
sayzyas 0:73dd48be5ca6 187 }
sayzyas 0:73dd48be5ca6 188 else
sayzyas 0:73dd48be5ca6 189 { if( cnt_mclock_LBCRW_r > 0 )
sayzyas 0:73dd48be5ca6 190 {
sayzyas 0:73dd48be5ca6 191 cnt_mclock_LBCRW_r -= 1;
sayzyas 0:73dd48be5ca6 192 }
sayzyas 0:73dd48be5ca6 193 else
sayzyas 0:73dd48be5ca6 194 {
sayzyas 0:73dd48be5ca6 195 cnt_mclock_LBCRW_r= 0;
sayzyas 0:73dd48be5ca6 196 }
sayzyas 0:73dd48be5ca6 197 }
sayzyas 0:73dd48be5ca6 198 if( cnt_mclock_LBCRW_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 199 {
sayzyas 0:73dd48be5ca6 200 flg_mclock_LBCRW = 1;
sayzyas 0:73dd48be5ca6 201 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 202 }
sayzyas 0:73dd48be5ca6 203 else
sayzyas 0:73dd48be5ca6 204 {
sayzyas 0:73dd48be5ca6 205 flg_mclock_LBCRW = 0;
sayzyas 0:73dd48be5ca6 206 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 207 }
sayzyas 0:73dd48be5ca6 208 }
sayzyas 0:73dd48be5ca6 209 }
sayzyas 0:73dd48be5ca6 210 // Check RF-Crawler
sayzyas 0:73dd48be5ca6 211 else if( motor_number == MOTOR_RFTFM )
sayzyas 0:73dd48be5ca6 212 {
sayzyas 0:73dd48be5ca6 213 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 214 {
sayzyas 0:73dd48be5ca6 215 mcchk = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_f);
sayzyas 0:73dd48be5ca6 216 if( mcchk == true )
sayzyas 0:73dd48be5ca6 217 {
sayzyas 0:73dd48be5ca6 218 cnt_mclock_RFTFM_f += 1;
sayzyas 0:73dd48be5ca6 219 }
sayzyas 0:73dd48be5ca6 220 else
sayzyas 0:73dd48be5ca6 221 { if( cnt_mclock_RFTFM_f > 0 )
sayzyas 0:73dd48be5ca6 222 {
sayzyas 0:73dd48be5ca6 223 cnt_mclock_RFTFM_f -= 1;
sayzyas 0:73dd48be5ca6 224 }
sayzyas 0:73dd48be5ca6 225 else
sayzyas 0:73dd48be5ca6 226 {
sayzyas 0:73dd48be5ca6 227 cnt_mclock_RFTFM_f = 0;
sayzyas 0:73dd48be5ca6 228 }
sayzyas 0:73dd48be5ca6 229 }
sayzyas 0:73dd48be5ca6 230 if( cnt_mclock_RFTFM_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 231 {
sayzyas 0:73dd48be5ca6 232 flg_mclock_RFTFM = 1;
sayzyas 0:73dd48be5ca6 233 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 234 }
sayzyas 0:73dd48be5ca6 235 else
sayzyas 0:73dd48be5ca6 236 {
sayzyas 0:73dd48be5ca6 237 flg_mclock_RFTFM = 0;
sayzyas 0:73dd48be5ca6 238 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 239 }
sayzyas 0:73dd48be5ca6 240 }
sayzyas 0:73dd48be5ca6 241 else
sayzyas 0:73dd48be5ca6 242 {
sayzyas 0:73dd48be5ca6 243 rts = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_r);
sayzyas 0:73dd48be5ca6 244 if( rts == true )
sayzyas 0:73dd48be5ca6 245 {
sayzyas 0:73dd48be5ca6 246 cnt_mclock_RFTFM_r += 1;
sayzyas 0:73dd48be5ca6 247 }
sayzyas 0:73dd48be5ca6 248 else
sayzyas 0:73dd48be5ca6 249 { if( cnt_mclock_RFTFM_r > 0 )
sayzyas 0:73dd48be5ca6 250 {
sayzyas 0:73dd48be5ca6 251 cnt_mclock_RFTFM_r -= 1;
sayzyas 0:73dd48be5ca6 252 }
sayzyas 0:73dd48be5ca6 253 else
sayzyas 0:73dd48be5ca6 254 {
sayzyas 0:73dd48be5ca6 255 cnt_mclock_RFTFM_r = 0;
sayzyas 0:73dd48be5ca6 256 }
sayzyas 0:73dd48be5ca6 257 }
sayzyas 0:73dd48be5ca6 258 if( cnt_mclock_RFTFM_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 259 {
sayzyas 0:73dd48be5ca6 260 flg_mclock_RFTFM = 1;
sayzyas 0:73dd48be5ca6 261 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 262 }
sayzyas 0:73dd48be5ca6 263 else
sayzyas 0:73dd48be5ca6 264 {
sayzyas 0:73dd48be5ca6 265 flg_mclock_RFTFM = 0;
sayzyas 0:73dd48be5ca6 266 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 267 }
sayzyas 0:73dd48be5ca6 268 }
sayzyas 0:73dd48be5ca6 269 }
sayzyas 0:73dd48be5ca6 270 // Check LB-Transform
sayzyas 0:73dd48be5ca6 271 else if( motor_number == MOTOR_LBTFM )
sayzyas 0:73dd48be5ca6 272 {
sayzyas 0:73dd48be5ca6 273 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 274 {
sayzyas 0:73dd48be5ca6 275 mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_f);
sayzyas 0:73dd48be5ca6 276 if( mcchk == true )
sayzyas 0:73dd48be5ca6 277 {
sayzyas 0:73dd48be5ca6 278 cnt_mclock_LBTFM_f += 1;
sayzyas 0:73dd48be5ca6 279 }
sayzyas 0:73dd48be5ca6 280 else
sayzyas 0:73dd48be5ca6 281 { if( cnt_mclock_LBTFM_f > 0 )
sayzyas 0:73dd48be5ca6 282 {
sayzyas 0:73dd48be5ca6 283 cnt_mclock_LBTFM_f -= 1;
sayzyas 0:73dd48be5ca6 284 }
sayzyas 0:73dd48be5ca6 285 else
sayzyas 0:73dd48be5ca6 286 {
sayzyas 0:73dd48be5ca6 287 cnt_mclock_LBTFM_f = 0;
sayzyas 0:73dd48be5ca6 288 }
sayzyas 0:73dd48be5ca6 289 }
sayzyas 0:73dd48be5ca6 290 if( cnt_mclock_LBTFM_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 291 {
sayzyas 0:73dd48be5ca6 292 flg_mclock_LBTFM = 1;
sayzyas 0:73dd48be5ca6 293 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 294 }
sayzyas 0:73dd48be5ca6 295 else
sayzyas 0:73dd48be5ca6 296 {
sayzyas 0:73dd48be5ca6 297 flg_mclock_LBTFM = 0;
sayzyas 0:73dd48be5ca6 298 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 299 }
sayzyas 0:73dd48be5ca6 300 }
sayzyas 0:73dd48be5ca6 301 else
sayzyas 0:73dd48be5ca6 302 {
sayzyas 0:73dd48be5ca6 303 mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_r);
sayzyas 0:73dd48be5ca6 304 if( mcchk == true )
sayzyas 0:73dd48be5ca6 305 {
sayzyas 0:73dd48be5ca6 306 cnt_mclock_LBTFM_r += 1;
sayzyas 0:73dd48be5ca6 307 }
sayzyas 0:73dd48be5ca6 308 else
sayzyas 0:73dd48be5ca6 309 { if( cnt_mclock_LBTFM_r > 0 )
sayzyas 0:73dd48be5ca6 310 {
sayzyas 0:73dd48be5ca6 311 cnt_mclock_LBTFM_r -= 1;
sayzyas 0:73dd48be5ca6 312 }
sayzyas 0:73dd48be5ca6 313 else
sayzyas 0:73dd48be5ca6 314 {
sayzyas 0:73dd48be5ca6 315 cnt_mclock_LBTFM_r = 0;
sayzyas 0:73dd48be5ca6 316 }
sayzyas 0:73dd48be5ca6 317 }
sayzyas 0:73dd48be5ca6 318 if( cnt_mclock_LBTFM_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 319 {
sayzyas 0:73dd48be5ca6 320 flg_mclock_LBTFM = 1;
sayzyas 0:73dd48be5ca6 321 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 322 }
sayzyas 0:73dd48be5ca6 323 else
sayzyas 0:73dd48be5ca6 324 {
sayzyas 0:73dd48be5ca6 325 flg_mclock_LBTFM = 0;
sayzyas 0:73dd48be5ca6 326 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 327 }
sayzyas 0:73dd48be5ca6 328 }
sayzyas 0:73dd48be5ca6 329 }
sayzyas 0:73dd48be5ca6 330
sayzyas 0:73dd48be5ca6 331 // Check Winch
sayzyas 0:73dd48be5ca6 332 else if( motor_number == MOTOR_WINCH )
sayzyas 0:73dd48be5ca6 333 {
sayzyas 0:73dd48be5ca6 334 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 335 {
sayzyas 0:73dd48be5ca6 336 mcchk= chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_f);
sayzyas 0:73dd48be5ca6 337 if( mcchk == true )
sayzyas 0:73dd48be5ca6 338 {
sayzyas 0:73dd48be5ca6 339 cnt_mclock_WINCH_f += 1;
sayzyas 0:73dd48be5ca6 340 }
sayzyas 0:73dd48be5ca6 341 else
sayzyas 0:73dd48be5ca6 342 { if( cnt_mclock_WINCH_f > 0 )
sayzyas 0:73dd48be5ca6 343 {
sayzyas 0:73dd48be5ca6 344 cnt_mclock_WINCH_f -= 1;
sayzyas 0:73dd48be5ca6 345 }
sayzyas 0:73dd48be5ca6 346 else
sayzyas 0:73dd48be5ca6 347 {
sayzyas 0:73dd48be5ca6 348 cnt_mclock_WINCH_f = 0;
sayzyas 0:73dd48be5ca6 349 }
sayzyas 0:73dd48be5ca6 350
sayzyas 0:73dd48be5ca6 351 }
sayzyas 0:73dd48be5ca6 352 if( cnt_mclock_WINCH_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 353 {
sayzyas 0:73dd48be5ca6 354 flg_mclock_WINCH = 1;
sayzyas 0:73dd48be5ca6 355 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 356 }
sayzyas 0:73dd48be5ca6 357 else
sayzyas 0:73dd48be5ca6 358 {
sayzyas 0:73dd48be5ca6 359 flg_mclock_WINCH = 0;
sayzyas 0:73dd48be5ca6 360 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 361 }
sayzyas 0:73dd48be5ca6 362 }
sayzyas 0:73dd48be5ca6 363 else
sayzyas 0:73dd48be5ca6 364 {
sayzyas 0:73dd48be5ca6 365 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_r);
sayzyas 0:73dd48be5ca6 366 if( mcchk == true )
sayzyas 0:73dd48be5ca6 367 {
sayzyas 0:73dd48be5ca6 368 cnt_mclock_WINCH_r += 1;
sayzyas 0:73dd48be5ca6 369 }
sayzyas 0:73dd48be5ca6 370 else
sayzyas 0:73dd48be5ca6 371 { if( cnt_mclock_WINCH_r > 0 )
sayzyas 0:73dd48be5ca6 372 {
sayzyas 0:73dd48be5ca6 373 cnt_mclock_WINCH_r -= 1;
sayzyas 0:73dd48be5ca6 374 }
sayzyas 0:73dd48be5ca6 375 else
sayzyas 0:73dd48be5ca6 376 {
sayzyas 0:73dd48be5ca6 377 cnt_mclock_WINCH_r = 0;
sayzyas 0:73dd48be5ca6 378 }
sayzyas 0:73dd48be5ca6 379 }
sayzyas 0:73dd48be5ca6 380 if( cnt_mclock_WINCH_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 381 {
sayzyas 0:73dd48be5ca6 382 flg_mclock_WINCH = 1;
sayzyas 0:73dd48be5ca6 383 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 384 }
sayzyas 0:73dd48be5ca6 385 else
sayzyas 0:73dd48be5ca6 386 {
sayzyas 0:73dd48be5ca6 387 flg_mclock_WINCH = 0;
sayzyas 0:73dd48be5ca6 388 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 389 }
sayzyas 0:73dd48be5ca6 390 }
sayzyas 0:73dd48be5ca6 391 }
sayzyas 0:73dd48be5ca6 392 // Check Tilt
sayzyas 0:73dd48be5ca6 393 else if( motor_number == MOTOR_CMPAN )
sayzyas 0:73dd48be5ca6 394 {
sayzyas 0:73dd48be5ca6 395 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 396 {
sayzyas 0:73dd48be5ca6 397 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_f);
sayzyas 0:73dd48be5ca6 398 if( mcchk == true )
sayzyas 0:73dd48be5ca6 399 {
sayzyas 0:73dd48be5ca6 400 cnt_mclock_CMPAN_f += 1;
sayzyas 0:73dd48be5ca6 401 }
sayzyas 0:73dd48be5ca6 402 else
sayzyas 0:73dd48be5ca6 403 { if( cnt_mclock_CMPAN_f > 0 )
sayzyas 0:73dd48be5ca6 404 {
sayzyas 0:73dd48be5ca6 405 cnt_mclock_CMPAN_f -= 1;
sayzyas 0:73dd48be5ca6 406 }
sayzyas 0:73dd48be5ca6 407 else
sayzyas 0:73dd48be5ca6 408 {
sayzyas 0:73dd48be5ca6 409 cnt_mclock_CMPAN_f = 0;
sayzyas 0:73dd48be5ca6 410 }
sayzyas 0:73dd48be5ca6 411
sayzyas 0:73dd48be5ca6 412 }
sayzyas 0:73dd48be5ca6 413 if( cnt_mclock_CMPAN_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 414 {
sayzyas 0:73dd48be5ca6 415 flg_mclock_CMPAN = 1;
sayzyas 0:73dd48be5ca6 416 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 417 }
sayzyas 0:73dd48be5ca6 418 else
sayzyas 0:73dd48be5ca6 419 {
sayzyas 0:73dd48be5ca6 420 flg_mclock_CMPAN = 0;
sayzyas 0:73dd48be5ca6 421 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 422 }
sayzyas 0:73dd48be5ca6 423 }
sayzyas 0:73dd48be5ca6 424 else
sayzyas 0:73dd48be5ca6 425 {
sayzyas 0:73dd48be5ca6 426 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_r);
sayzyas 0:73dd48be5ca6 427 if( mcchk == true )
sayzyas 0:73dd48be5ca6 428 {
sayzyas 0:73dd48be5ca6 429 cnt_mclock_CMPAN_r += 1;
sayzyas 0:73dd48be5ca6 430 }
sayzyas 0:73dd48be5ca6 431 else
sayzyas 0:73dd48be5ca6 432 { if( cnt_mclock_CMPAN_r > 0 )
sayzyas 0:73dd48be5ca6 433 {
sayzyas 0:73dd48be5ca6 434 cnt_mclock_CMPAN_r -= 1;
sayzyas 0:73dd48be5ca6 435 }
sayzyas 0:73dd48be5ca6 436 else
sayzyas 0:73dd48be5ca6 437 {
sayzyas 0:73dd48be5ca6 438 cnt_mclock_CMPAN_r = 0;
sayzyas 0:73dd48be5ca6 439 }
sayzyas 0:73dd48be5ca6 440 }
sayzyas 0:73dd48be5ca6 441 if( cnt_mclock_CMPAN_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 442 {
sayzyas 0:73dd48be5ca6 443 flg_mclock_CMPAN = 1;
sayzyas 0:73dd48be5ca6 444 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 445 }
sayzyas 0:73dd48be5ca6 446 else
sayzyas 0:73dd48be5ca6 447 {
sayzyas 0:73dd48be5ca6 448 flg_mclock_CMPAN = 0;
sayzyas 0:73dd48be5ca6 449 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 450 }
sayzyas 0:73dd48be5ca6 451 }
sayzyas 0:73dd48be5ca6 452 }
sayzyas 0:73dd48be5ca6 453 // Check Tilt
sayzyas 0:73dd48be5ca6 454 else if( motor_number == MOTOR_CTILT )
sayzyas 0:73dd48be5ca6 455 {
sayzyas 0:73dd48be5ca6 456 if( motor_dir == MOTOR_DIR_FWD )
sayzyas 0:73dd48be5ca6 457 {
sayzyas 0:73dd48be5ca6 458 mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_f);
sayzyas 0:73dd48be5ca6 459 if( mcchk == true )
sayzyas 0:73dd48be5ca6 460 {
sayzyas 0:73dd48be5ca6 461 cnt_mclock_CTILT_f += 1;
sayzyas 0:73dd48be5ca6 462 }
sayzyas 0:73dd48be5ca6 463 else
sayzyas 0:73dd48be5ca6 464 { if( cnt_mclock_CTILT_f > 0 )
sayzyas 0:73dd48be5ca6 465 {
sayzyas 0:73dd48be5ca6 466 cnt_mclock_CTILT_f -= 1;
sayzyas 0:73dd48be5ca6 467 }
sayzyas 0:73dd48be5ca6 468 else
sayzyas 0:73dd48be5ca6 469 {
sayzyas 0:73dd48be5ca6 470 cnt_mclock_CTILT_f = 0;
sayzyas 0:73dd48be5ca6 471 }
sayzyas 0:73dd48be5ca6 472 }
sayzyas 0:73dd48be5ca6 473 if( cnt_mclock_CTILT_f > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 474 {
sayzyas 0:73dd48be5ca6 475 flg_mclock_CTILT = 1;
sayzyas 0:73dd48be5ca6 476 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 477 }
sayzyas 0:73dd48be5ca6 478 else
sayzyas 0:73dd48be5ca6 479 {
sayzyas 0:73dd48be5ca6 480 flg_mclock_CTILT = 0;
sayzyas 0:73dd48be5ca6 481 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 482 }
sayzyas 0:73dd48be5ca6 483 }
sayzyas 0:73dd48be5ca6 484 else
sayzyas 0:73dd48be5ca6 485 {
sayzyas 0:73dd48be5ca6 486 mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_r);
sayzyas 0:73dd48be5ca6 487 if( mcchk == true )
sayzyas 0:73dd48be5ca6 488 {
sayzyas 0:73dd48be5ca6 489 cnt_mclock_CTILT_r += 1;
sayzyas 0:73dd48be5ca6 490 }
sayzyas 0:73dd48be5ca6 491 else
sayzyas 0:73dd48be5ca6 492 { if( cnt_mclock_CTILT_r > 0 )
sayzyas 0:73dd48be5ca6 493 {
sayzyas 0:73dd48be5ca6 494 cnt_mclock_CTILT_r -= 1;
sayzyas 0:73dd48be5ca6 495 }
sayzyas 0:73dd48be5ca6 496 else
sayzyas 0:73dd48be5ca6 497 {
sayzyas 0:73dd48be5ca6 498 cnt_mclock_CTILT_r = 0;
sayzyas 0:73dd48be5ca6 499 }
sayzyas 0:73dd48be5ca6 500 }
sayzyas 0:73dd48be5ca6 501 if( cnt_mclock_CTILT_r > motor_lock_chk_cnt )
sayzyas 0:73dd48be5ca6 502 {
sayzyas 0:73dd48be5ca6 503 flg_mclock_CTILT = 1;
sayzyas 0:73dd48be5ca6 504 rts = true; // LOCK
sayzyas 0:73dd48be5ca6 505 }
sayzyas 0:73dd48be5ca6 506 else
sayzyas 0:73dd48be5ca6 507 {
sayzyas 0:73dd48be5ca6 508 flg_mclock_CTILT = 0;
sayzyas 0:73dd48be5ca6 509 rts = false; // LOCK
sayzyas 0:73dd48be5ca6 510 }
sayzyas 0:73dd48be5ca6 511 }
sayzyas 0:73dd48be5ca6 512 }
sayzyas 0:73dd48be5ca6 513 return rts;
sayzyas 0:73dd48be5ca6 514 }
sayzyas 0:73dd48be5ca6 515
sayzyas 0:73dd48be5ca6 516 bool mcchk::chk_motor_lock(
sayzyas 0:73dd48be5ca6 517 float motor_current, // motor current
sayzyas 0:73dd48be5ca6 518 float motor_center_value, // motor current center value
sayzyas 0:73dd48be5ca6 519 int mc_threshold // motor current threshold
sayzyas 0:73dd48be5ca6 520 ){
sayzyas 0:73dd48be5ca6 521 // int32_t mc_abs_pct;
sayzyas 0:73dd48be5ca6 522
sayzyas 0:73dd48be5ca6 523 _cnt_now = motor_current;
sayzyas 0:73dd48be5ca6 524 _cnt_center = motor_center_value;
sayzyas 0:73dd48be5ca6 525 _cnt_th = mc_threshold;
sayzyas 0:73dd48be5ca6 526
sayzyas 0:73dd48be5ca6 527 mc_abs_pct = abs((int32_t)((motor_current - motor_center_value)*100.0f));
sayzyas 0:73dd48be5ca6 528 if( mc_abs_pct > mc_threshold )
sayzyas 0:73dd48be5ca6 529 {
sayzyas 0:73dd48be5ca6 530 return true; // Motor current is over threshold.
sayzyas 0:73dd48be5ca6 531 }
sayzyas 0:73dd48be5ca6 532 else
sayzyas 0:73dd48be5ca6 533 {
sayzyas 0:73dd48be5ca6 534 return false; // Motor current isn't over threshold.
sayzyas 0:73dd48be5ca6 535 }
sayzyas 0:73dd48be5ca6 536 }