z ysaito / Mbed 2 deprecated CPro_DemoB1B2_4_ResAnaCtrl

2018.08.03 1st commit of demo main res ctrl

Dependencies:   QEI mbed

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

Committer:
sayzyas
Date:
Thu Jul 26 00:19:47 2018 +0000
Revision:
0:f8373bf202a6
2018.07.26

Who changed what in which revision?

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