/* * WESPION_App.c * * Revised on: Dec 17, 2023 * Author: HwangSungWoo * * 1. 가동범위 저점 설정 방식 변경 */ #include "WESPION.h" #include "main.h" #include "led.h" /* ISR 수행시간 프로파일링 (v1.0.2) * DWT Cycle Counter 사용. 측정 오버헤드 ~2 클럭 (11ns). */ ISR_ProfileTypeDef ISR_Profile = {0}; void ISR_Profile_Init(void) { CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; DWT->CYCCNT = 0; DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; } void ISR_Profile_Reset(void) { ISR_Profile.max = 0; ISR_Profile.avg = 0; ISR_Profile.count = 0; } /* BLE Debug Report (v1.0.2) * 1초 주기로 ISR 프로파일링 데이터를 BLE로 전송. * 앱에서 DEBUG_REPORT(0xF5) 패킷으로 수신. * * 패킷 구조 (12 bytes): * [FF FF] [09] [F5] [avg_H avg_L] [max_H max_L] [usage] [state] [skip] [CHK] * avg: ISR 평균 사이클/18 = 0.1µs 단위 (uint16, 표시 시 /10) * max: ISR 최대 사이클/18 = 0.1µs 단위 (uint16, 표시 시 /10) * usage: max / 36000 * 100 = % (uint8, 0~100) * state: Drive_Status (uint8) * skip: Debug_SkipEncInit (uint8, 0=normal, 1=skipped) */ uint8_t Debug_Report_Active = 0; // 앱에서 활성화 토글 /* Debug Report 패킷 레이아웃 (v1.0.4) * ───────────────────────────────────── * [0-1] FF FF 헤더 * [2] size 페이로드 길이 (헤더 제외) * [3] F5 커맨드 코드 * --- ISR 프로파일링 (기존) --- * [4-5] avg_us ISR 평균 (0.1µs 단위) * [6-7] max_us ISR 최대 (0.1µs 단위) * [8] usage ISR 사용률 (%) * [9] drive_status Drive 상태 * [10] skip_enc_init EncInit 우회 여부 * --- HIL 검증 데이터 (신규) --- * [11-12] posL PositionFine[L] (0.05mm, int16) * [13-14] posR PositionFine[R] (0.05mm, int16) * [15-16] spdL Speed[L] (mm/s, int16) * [17-18] spdR Speed[R] (mm/s, int16) * [19-20] iqL TargetIq[L] × 1000 (0.001A, int16) * [21-22] iqR TargetIq[R] × 1000 (0.001A, int16) * [23] modeL WeightMode[L] * [24] modeR WeightMode[R] * [25] regionL RegionCurr[L] * [26] regionR RegionCurr[R] * --- 끝 --- * [27] checksum -(sum of all bytes) & 0xFF * 총 28바이트 */ void Return_Debug_Report(void) { if(!Debug_Report_Active) return; uint16_t avg_us = (uint16_t)(ISR_Profile.avg / 18); uint16_t max_us = (uint16_t)(ISR_Profile.max / 18); uint8_t usage = (uint8_t)((float)ISR_Profile.max / 36000.0f * 100.0f); int16_t posL = (int16_t)WP_Gym.PositionFine[L]; int16_t posR = (int16_t)WP_Gym.PositionFine[R]; int16_t spdL = (int16_t)WP_Gym.Speed[L]; int16_t spdR = (int16_t)WP_Gym.Speed[R]; int16_t iqL = (int16_t)(TargetIq[L] * 1000); int16_t iqR = (int16_t)(TargetIq[R] * 1000); uint8_t data[3+25]; // 28바이트 data[0] = 0xFF; data[1] = 0xFF; data[2] = sizeof(data) - 3; // 25 data[3] = DEBUG_REPORT; // 0xF5 // ISR 프로파일링 data[4] = (uint8_t)(avg_us >> 8); data[5] = (uint8_t)(avg_us); data[6] = (uint8_t)(max_us >> 8); data[7] = (uint8_t)(max_us); data[8] = usage; data[9] = Drive_Status; data[10] = Debug_SkipEncInit; // HIL 검증 데이터 data[11] = (uint8_t)(posL >> 8); data[12] = (uint8_t)(posL); data[13] = (uint8_t)(posR >> 8); data[14] = (uint8_t)(posR); data[15] = (uint8_t)(spdL >> 8); data[16] = (uint8_t)(spdL); data[17] = (uint8_t)(spdR >> 8); data[18] = (uint8_t)(spdR); data[19] = (uint8_t)(iqL >> 8); data[20] = (uint8_t)(iqL); data[21] = (uint8_t)(iqR >> 8); data[22] = (uint8_t)(iqR); data[23] = WP_Gym.WeightMode[L]; data[24] = WP_Gym.WeightMode[R]; data[25] = WP_Gym.RegionCurr[L]; data[26] = WP_Gym.RegionCurr[R]; // Checksum data[27] = 0; for(uint8_t i = 0; i < sizeof(data) - 1; i++) { data[27] += data[i]; } data[27] = -data[27]; UART3_PutData(data, sizeof(data)); Return_LineEnding(); ISR_Profile.max = 0; } /* ISR → Scheduler 지연 알림 (v1.0.2) * ISR에서 플래그만 세팅, 여기서 실제 UART/BLE 전송 수행. * 스케줄러 5ms 슬롯에서 호출됨. */ ISR_DeferredTypeDef ISR_Deferred = {0}; void ISR_DeferredNotify(void) { // 1. WeightOnOff 완료 알림 if (ISR_Deferred.WeightOnOff_Ready) { Return_1byte(WEIGHTONOFF, ISR_Deferred.WeightOnOff_Status); ISR_Deferred.WeightOnOff_Ready = 0; } // 2. MotionAutoWeight 발동 알림 if (ISR_Deferred.MotionAuto_Ready) { Return_2x1byte(AUTOWEIGHT_STATUS, ISR_Deferred.MotionAuto_StatusL, ISR_Deferred.MotionAuto_StatusR); ISR_Deferred.MotionAuto_Ready = 0; } // 3. ClampEccWeight 변경 알림 if (ISR_Deferred.EccClamp_Ready) { Return_4x1byte(ECC_LEVEL, ISR_Deferred.EccClamp_L, ISR_Deferred.EccClamp_R, ISR_Deferred.EccClamp_Err, 0); ISR_Deferred.EccClamp_Ready = 0; } } // 상위 제어 기능 //int cnt_WeightManager, check_Iq_sign_Err = 0; //int debug_FF_Region[2] = {0,0}; float TargetIq[2]; // ── Dev Report 전역 변수 ── float DevReport_FLoad[2] = {0}; // 계산된 부하 (kg) float DevReport_Accel[2] = {0}; // 가속도 (mm/s²) static float DevReport_PrevSpd[2] = {0}; // 가속도 계산용 이전 속도 uint8_t DevReport_Region[2] = {0}; // 위치 영역 //int Region_L, Region_R; void WeightManager(void) { // cnt_WeightManager++; switch (Debug_DriveMotor) { case 3: WP_Ctrl.Mode = Inv_TorqueCtrl; WP_Ctrl.ActiveMotor = 3; // DropRelease(); // PullWeightOn(); MotionAutoWeight(); WeightController(L); WeightController(R); // 기능 테스트용 안전 모드 Motor1.Cmd.Icmd.q = TargetIq[L]; Motor2.Cmd.Icmd.q = TargetIq[R]; break; case 0: // 양쪽 정지 // WP_Ctrl.Mode = Inv_Off; // WP_Ctrl.ActiveMotor = 0; // WP_Gym.WeightSet[L] = 0; // WP_Gym.WeightSet[R] = 0; // Motor1.Cmd.Icmd.q = WP_Gym.F_stop; // Motor2.Cmd.Icmd.q = WP_Gym.F_stop; break; case 1: WP_Ctrl.Mode = Inv_TorqueCtrl; WP_Ctrl.ActiveMotor = 1; WP_Gym.WeightSet[R] = 0; WeightController(L); Motor1.Cmd.Icmd.q = TargetIq[L]; // Motor2.Cmd.Icmd.q = WP_Gym.F_min; break; case 2: WP_Ctrl.Mode = Inv_TorqueCtrl; WP_Ctrl.ActiveMotor = 2; WP_Gym.WeightSet[L] = 0; WeightController(R); // Motor1.Cmd.Icmd.q = WP_Gym.F_min; Motor2.Cmd.Icmd.q = TargetIq[R]; break; default: WP_Ctrl.Mode = Inv_Off; WP_Ctrl.ActiveMotor = 0; WP_Weight.Ctrl.Target[L] = 0; WP_Weight.Ctrl.Target[R] = 0; // Motor1.Cmd.Icmd.q = 0; // Motor2.Cmd.Icmd.q = 0; break; } } //int cnt_WeightController; void WeightController (int side) { // ScaleWeight2Current가 반영 안된 상태임!! // cnt_WeightController++; WeightOnOff(side); ClampEccWeight(side); static float F_Load_prev[2] = {0}; // Slew Rate Limit용 static float F_Load_iso[2] = {0}; // 등속성 적분 상태 변수 float F_Load, SettingWeight, TargetWeight, Iq_Temp, Spd; float FeccTemp = 0; uint8_t Mode, Region; int16_t Pos, H_LoSoft, RangeLo, RangeHi, H_HiSoft; // 굳이 좌우 변수를 지역변수에 할당해야만 하는 건 아니지만, 로직 부분 함수 가독성 개선을 위해 짧게 수정하려고 살려둠 SettingWeight = WP_Gym.WeightSet[side]; TargetWeight = SettingWeight * WP_Gym.AutoWeightRatio[side]; FeccTemp = WP_Weight.Ctrl.Ecc[side]; Mode = WP_Gym.WeightMode[side]; // Pos = WP_Gym.Position[side]; Pos = WP_Gym.PositionFine2[side]; // 0.1cm 단위 Spd = WP_Gym.Speed[side]; H_LoSoft = WP_Gym.Region.H_LoSoft[side]; // 모두 1cm 단위 RangeLo = WP_Gym.Region.RangeLo[side]; RangeHi = WP_Gym.Region.RangeHi[side]; H_HiSoft = WP_Gym.Region.H_HiSoft[side]; // 0.1mm 단위로 역(10배) 환산 if (-32768 <= Pos && Pos < WP_Gym.Region.H_gnd *10) { Iq_Temp = WP_Gym.F_stop * WP_Machine.Scale_Weight2Current; Region = Block; } else if(WP_Gym.Region.H_gnd*10 <= Pos && Pos < WP_Gym.Region.H_base*10) { Iq_Temp = WP_Gym.F_min * WP_Machine.Scale_Weight2Current; Region = Ground; } else if(WP_Gym.Region.H_base*10 <= Pos && Pos < WP_Gym.Region.H_shock*10) { // Iq_Temp = WP_Gym.F_shock; Iq_Temp = LINEAR_INTERPOLATION(Pos, WP_Gym.Region.H_base*10, WP_Gym.Region.H_shock*10, WP_Gym.F_min, WP_Gym.F_idle) * WP_Machine.Scale_Weight2Current; Region = Base; } else if(WP_Gym.Region.H_shock*10 <= Pos && Pos < H_LoSoft*10) { Iq_Temp = WP_Gym.F_idle * WP_Machine.Scale_Weight2Current; Region = Idle; } else if(H_LoSoft*10 <= Pos && Pos < 32767){ switch(Mode){ case 0: // 모드 0: 일반 부하 // 노미널 부하 계산 FeccTemp = 0; F_Load = TargetWeight * WP_Weight.Ctrl.OnOffScale[side]; if(F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; if(H_LoSoft*10 <= Pos && Pos < RangeLo*10) { Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; Region = LoSoft; } else if(RangeLo*10 <= Pos){ Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = RoM; } // 이후 구간은 안전문제 판단이 미흡해서 미구현 break; case 1: // 모드 1: 신장성 모드 // 노미널 부하 계산 if (WP_Gym.EccSpdUp < Spd) { // 속도가 상한 이상인 경우 = 올라감 -> 기본 무게 (올라가는 방향이 +) FeccTemp = FeccTemp - WP_Weight.Ecc_Step; WP_LEDCtrl.R = 20; WP_LEDCtrl.G = 4; WP_LEDCtrl.B = 80; if(FeccTemp <= 0){ FeccTemp = 0; } } else if (Spd <= WP_Gym.EccSpdDown) { // 속도가 하한 이하인 경우 = 내려감 -> 더해진 무게 FeccTemp = FeccTemp + WP_Weight.Ecc_Step; WP_LEDCtrl.R = 40; WP_LEDCtrl.G = 4; WP_LEDCtrl.B = 60; if(FeccTemp >= WP_Gym.F_EccSet){ FeccTemp = WP_Gym.F_EccSet; } } F_Load = (TargetWeight + FeccTemp) * WP_Weight.Ctrl.OnOffScale[side]; if(F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; if(H_LoSoft*10 <= Pos && Pos < RangeLo*10) { Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; Region = LoSoft; } else if(RangeLo*10 <= Pos){ Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = RoM; } // 이후 구간은 안전문제 판단이 미흡해서 미구현 break; // case 2: // 모드 2: 단축성 (포지티브) 모드 // // 노미널 부하 계산 // if (WP_Gym.EccSpdUp < Spd) { // 속도가 상한 이상인 경우 = 올라감 -> 더해진 무게 // FeccTemp = FeccTemp + WP_Weight.Ecc_Step; // LED_Weight_R = 40; // LED_Weight_G = 4; // LED_Weight_B = 60; // if (FeccTemp >= WP_Gym.F_EccSet) { // FeccTemp가 너무 커지지 않도록 제어 // FeccTemp = WP_Gym.F_EccSet; // } // } else if (Spd <= WP_Gym.EccSpdDown) { // 속도가 하한 이하인 경우 = 내려감 -> 기본 무게로 복귀 // FeccTemp = FeccTemp - WP_Weight.Ecc_Step; // LED_Weight_R = 20; // LED_Weight_G = 4; // LED_Weight_B = 80; // if (FeccTemp <= 0) { // FeccTemp가 음수로 내려가지 않도록 제어 // FeccTemp = 0; // } // } // // 최종 부하 계산 (올라갈 때 무게 증가, 내려갈 때 기본 무게) // F_Load = (TargetWeight + FeccTemp) * WP_Weight.Ctrl.OnOffScale[side]; // if (F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; // // // 위치에 따른 부하 제어 // if (H_LoSoft*10 <= Pos && Pos < RangeLo*10) { // Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; // Region = LoSoft; // } else if (RangeLo*10 <= Pos) { // Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; // Region = RoM; // } // break; case 2: // 모드 2: 밴드 모드 // 노미널 부하 계산 FeccTemp = 0; F_Load = TargetWeight * WP_Weight.Ctrl.OnOffScale[side]; if(F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; if(H_LoSoft*10 <= Pos && Pos < RangeHi*10){ Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; WP_LEDCtrl.R = 10 + 240 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10); if(WP_LEDCtrl.R >= 255) WP_LEDCtrl.R = 255; // if (Pos >= 600) { // LED_Weight_G = 120 - 120 * (Pos - H_LoSoft - 600) / (RangeHi - H_LoSoft - 600); // } WP_LEDCtrl.G = 40 - 35 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10); // if(LED_Weight_G <= 10) LED_Weight_G = 10; WP_LEDCtrl.B = 40 - 40 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10); // if(LED_Weight_B <= 10) LED_Weight_B = 10; Region = RoM; // 밴드모드는 3이 따로 없고 4로 통합임 } else if(RangeHi*10 <= Pos && Pos < 32767){ Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = Over; } break; case 3: // 모드 3: 등속성 모드 (Isokinetic) — PI 피드백 제어 // P항: 속도 오차에 즉각 반응 (빠른 응답) // I항: 잔여 오차 누적 제거 (정밀 유지) FeccTemp = 0; { float v_target = WP_Weight.Iso.Vtarget; float kp = WP_Weight.Iso.Kp; float ki = WP_Weight.Iso.Ki; float dt = 0.0001f; // ISR 10kHz = 0.1ms float deadband = 35.0f; // mm/s — 방향 전환 구간 데드밴드 float fmin = WP_Weight.Iso.Fmin > WP_Gym.F_idle ? WP_Weight.Iso.Fmin : WP_Gym.F_idle; float neutral = fmin; // 최소 텐션 (Fmin or F_idle 중 큰 쪽) if (Spd > deadband) { // ── 상승: PI 제어 활성 ── float error = Spd - v_target; // +: 빠름, -: 느림 float vtol = v_target * 0.05f; if (error > vtol || error < -vtol) { if (error > 0) { F_Load_iso[side] += ki * 3.0f * error * dt; // 브레이킹 3배 } else { F_Load_iso[side] += ki * error * dt; // 풀기 1배 } } // P항 float F_p; if (error > 0) { F_p = kp * 3.0f * error; } else { F_p = kp * error; } float fmax = (WP_Weight.Iso.Fmax > 0) ? WP_Weight.Iso.Fmax : TargetWeight; float F_pi = F_Load_iso[side] + F_p; if (F_pi > fmax) F_pi = fmax; if (F_pi < fmin) F_pi = fmin; F_Load = F_pi * WP_Weight.Ctrl.OnOffScale[side]; } else if (Spd < -deadband) { // ── 하강: 제어 없음, I항 중립으로 빠르게 감쇠 ── float decayRate = WP_Weight.Iso.DecayRate * dt; if (F_Load_iso[side] > neutral) { F_Load_iso[side] -= decayRate; if (F_Load_iso[side] < neutral) F_Load_iso[side] = neutral; } else if (F_Load_iso[side] < neutral) { F_Load_iso[side] += decayRate; if (F_Load_iso[side] > neutral) F_Load_iso[side] = neutral; } F_Load = F_Load_iso[side] * WP_Weight.Ctrl.OnOffScale[side]; } else { // ── 정지: I항 중립으로 감쇠 ── float decayRate = (WP_Weight.Iso.DecayRate * 0.5f) * dt; // 하강의 절반 속도 if (F_Load_iso[side] > neutral) { F_Load_iso[side] -= decayRate; if (F_Load_iso[side] < neutral) F_Load_iso[side] = neutral; } else if (F_Load_iso[side] < neutral) { F_Load_iso[side] += decayRate; if (F_Load_iso[side] > neutral) F_Load_iso[side] = neutral; } F_Load = F_Load_iso[side] * WP_Weight.Ctrl.OnOffScale[side]; } // I항 자체도 클램프 (와인드업 방지) { float fmax_clamp = (WP_Weight.Iso.Fmax > 0) ? WP_Weight.Iso.Fmax : TargetWeight; if (F_Load_iso[side] > fmax_clamp) F_Load_iso[side] = fmax_clamp; } if (F_Load_iso[side] < fmin) F_Load_iso[side] = fmin; if (F_Load < fmin) F_Load = fmin; // Slew Rate Limit float maxDelta = WP_Weight.Safety.SlewRate_kgPerSec * dt; float delta = F_Load - F_Load_prev[side]; if (delta > maxDelta) F_Load = F_Load_prev[side] + maxDelta; else if (delta < -maxDelta) F_Load = F_Load_prev[side] - maxDelta; F_Load_prev[side] = F_Load; } if (H_LoSoft*10 <= Pos && Pos < RangeLo*10) { Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; Region = LoSoft; } else if (RangeLo*10 <= Pos) { Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = RoM; } break; case 4: // 모드 4: 유체저항 모드 (Hydraulic) // F = Fmin + (W - Fmin) × (v / Vmax)^n — Vmax에서 W 도달 FeccTemp = 0; { // 하강(Spd < 0) 시 부하 감소: spdRatio=0 → minRatio만 적용 float upSpd = (Spd > 0) ? Spd : 0; float cappedSpd = upSpd; if (cappedSpd > WP_Weight.Hydro.Vmax) cappedSpd = WP_Weight.Hydro.Vmax; float vmax = WP_Weight.Hydro.Vmax; float n = WP_Weight.Hydro.n; float minR = WP_Weight.Hydro.minRatio; // powf 회피: n=1.0이면 선형, 아니면 powf float spdRatio; if (n == 1.0f) { spdRatio = cappedSpd / vmax; } else { spdRatio = powf(cappedSpd / vmax, n); } F_Load = TargetWeight * (minR + (1.0f - minR) * spdRatio) * WP_Weight.Ctrl.OnOffScale[side]; if (F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; // Slew Rate Limit float dt = 0.0002f; float maxDelta = WP_Weight.Safety.SlewRate_kgPerSec * dt; float delta = F_Load - F_Load_prev[side]; if (delta > maxDelta) F_Load = F_Load_prev[side] + maxDelta; else if (delta < -maxDelta) F_Load = F_Load_prev[side] - maxDelta; F_Load_prev[side] = F_Load; } if (H_LoSoft*10 <= Pos && Pos < RangeLo*10) { Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; Region = LoSoft; } else if (RangeLo*10 <= Pos) { Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = RoM; } break; case 5: // 모드 5: 진동 모드 (Vibration) — 재활/근활성화 // F = TargetWeight + clamp(vibKg, absMax) × sin(2π × freq × t) FeccTemp = 0; { float t_sec = (float)Get_Time() / 1000.0f; float osc = sinf(2.0f * 3.14159265f * WP_Weight.Vib.Freq * t_sec); // 진폭 계산: kg 직접 + 비율 안전 클램프 float vibKg = WP_Weight.Vib.AmplitudeKg; float maxByRatio = TargetWeight * WP_Weight.Vib.MaxRatio; if (vibKg > maxByRatio) vibKg = maxByRatio; F_Load = (TargetWeight + vibKg * osc) * WP_Weight.Ctrl.OnOffScale[side]; if (F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle; // Slew Rate Limit float dt = 0.0002f; float maxDelta = WP_Weight.Safety.SlewRate_kgPerSec * dt; float delta = F_Load - F_Load_prev[side]; if (delta > maxDelta) F_Load = F_Load_prev[side] + maxDelta; else if (delta < -maxDelta) F_Load = F_Load_prev[side] - maxDelta; F_Load_prev[side] = F_Load; } if (H_LoSoft*10 <= Pos && Pos < RangeLo*10) { Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current; Region = LoSoft; } else if (RangeLo*10 <= Pos) { Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current; Region = RoM; } break; default: F_Load = WP_Gym.F_idle; break; } } // 부하 조건에 따라서 피드포워드 스케일 타겟 계산 float Ff_Scale = 0; /* * 저점 이하에서 토크 제어 이상한 원인 찾았다!!! 단위 바뀌면서 *10해줘야 하는데 위에만 하고 여기 빼먹었었음!! (250705) * 가 아니라 왜 저점 아래에선 전향보상을 안 하는 거지? *10을 빼먹는 바람에 이 구분에 따른 아래 영역이, 실제 저점을 꽤 높게 설정하지 않으면 체감되지 어려워서 다행이었던 것 같음! * (저점 이하에서도 크게 설정해야 하는 거 아닌가?) * 가 절대 아니고, 이러면 바닥에 떨궜을 때 지혼자 진동 발산하는 중대 문제 발생!!! * * 아 이제 알겠다! * 바닥에 쾅 내려 놓을 때가 문제!? * * 하강 방향 시 0으로 두는 게 여태 가장 좋아보임!! 그래도 혹시 모르니 v1.0.1에는 미반영!!!! * 예) if(Spd < -150) Ff_Scale = 0.0f;; * * 다 아니고 그냥 구간 설정이 문제였다고 봄 * - H_Shock 이하는 항상 0으로 둬서 타겟 전류가 0(거의)인데도 불구 전류 지령이 흔들리는 걸 방지하고 * - 그 위부터 저점까지는 저점을 어떻게 설정하든 항상 0ff 상태 무게값에 적합한 스케일 * - 저점 위는 그대로 유지 * 지극히 당연함. -> 반영 하자!! * * */ #if 0 // R2edit : 미니귀뚜라미 제거 목적 테스트 -> 아무 차이 없 switch(Pos){ case Over: case HiSoft: case RoM: case LoSoft: if (F_Load <= 1.5) { Ff_Scale = WP_Weight.FfScale.Max; // debug_FF_Region[side] = 1; } else if (1.5 < F_Load && F_Load <= 2.5) { // Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min); // 왠지 모르게 힘이 튁 튕김 // Ff_Scale = 0.8f; Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // debug_FF_Region[side] = 2; } else if(2.5 < F_Load) { Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 3; } break; case Idle: Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // 여기에 속도 기반 결정 로직을 추가 // 빠르면 강하게, 느리면 약하게 break; case Base: case Ground: case Block: default: Ff_Scale = 0.0f; break; } #endif #if 1 // R2 if(Pos >= RangeLo*10) { if (F_Load <= 1.5) { Ff_Scale = WP_Weight.FfScale.Max; // debug_FF_Region[side] = 1; } else if (1.5 < F_Load && F_Load <= 2.5) { // Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min); // 왠지 모르게 힘이 튁 튕김 // Ff_Scale = 0.8f; Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // debug_FF_Region[side] = 2; } else if(2.5 < F_Load) { Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 3; } } else if(RangeLo*10 > Pos && Pos > WP_Gym.Region.H_shock*10) { Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // 여기에 속도 기반 결정 로직을 추가 // 빠르면 강하게, 느리면 약하게 } else if(WP_Gym.Region.H_shock*10 > Pos){ Ff_Scale = 0.0f; } #endif #if 0 // R1 : 미완이지만 이게 더 안정해보이긴 함. 그래도 검증 안 됐으니 미적용하고 배포하는 게 안전?? (250705) if (Pos >= WP_Gym.Region.H_shock*10) { if (F_Load <= 1.5) { Ff_Scale = WP_Weight.FfScale.Max; // debug_FF_Region[side] = 1; } else if (1.5 < F_Load && F_Load <= 2.5) { // Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min); // 왠지 모르게 힘이 튁 튕김 // Ff_Scale = 0.8f; Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // debug_FF_Region[side] = 2; } else if(2.5 < F_Load) { Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 3; } } else { Ff_Scale = 0.0f; // Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 4; } #endif #if 0 /* 원본 (초도 출고) */ if (Pos >= RangeLo) { if (F_Load <= 1.5) { Ff_Scale = WP_Weight.FfScale.Max; // debug_FF_Region[side] = 1; } else if (1.5 < F_Load && F_Load <= 2.5) { // Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min); // 왠지 모르게 힘이 튁 튕김 // Ff_Scale = 0.8f; Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5; // debug_FF_Region[side] = 2; } else if(2.5 < F_Load) { Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 3; } } else { Ff_Scale = 0.0f; // Ff_Scale = WP_Weight.FfScale.Min; // debug_FF_Region[side] = 4; } #endif FfScaleChanger(side, Ff_Scale); // 실제 전류 지령에 반영, 파라미터 갱신 // 부하 상한 (kg 기준) — 어떤 모드든 이 무게 이상 부하 불가 // 소프트 Iq 상한 (앱에서 설정, 0이면 비활성) if(WP_Weight.Safety.IqLimit > 0 && Iq_Temp > WP_Weight.Safety.IqLimit) Iq_Temp = WP_Weight.Safety.IqLimit; if(Iq_Temp >= WP_Machine.MaxCurrent) Iq_Temp = WP_Machine.MaxCurrent; TargetIq[side] = Iq_Temp; WP_Gym.RegionCurr[side] = Region; WP_Weight.Ctrl.Ecc[side] = FeccTemp; // Dev Report용 전역 저장 DevReport_FLoad[side] = Iq_Temp / WP_Machine.Scale_Weight2Current; // Iq→kg 역변환 DevReport_Region[side] = Region; } //void DirectionDetector (int side) { // float Spd; // int8_t Status; // // switch(side){ // case L: // Spd = WP_Gym.Speed[L]; // break; // case R: // Spd = WP_Gym.Speed[R]; // break; // } // // if(WP_Weight.RefSpdMove < Spd){ // Status = UP; // } else if(-WP_Weight.RefSpdMove < Spd && Spd <= WP_Weight.RefSpdMove){ // Status = STOP; // } // else if(Spd <= -WP_Weight.RefSpdMove) { // Status = DOWN; // } // // WP_Weight.Dir.Status[side] = Status; //} //int cnt_Ff_LimitHi, cnt_Ff_LimitLo; void FfScaleChanger (int side, float FfScale_Target) { float FfScale_new, FfScale_old, FfScaleDiff; FfScale_old = WP_Weight.FfScale.Temp[side]; // 절대값으로 변경 -> 액티브하지 않지만 확실 if (FfScale_old > FfScale_Target) { FfScale_new = FfScale_old - WP_Weight.FfScale.Step; if(FfScale_new >= WP_Weight.FfScale.Max) { // cnt_Ff_LimitHi++; FfScale_new = WP_Weight.FfScale.Max; } } else if (FfScale_old < FfScale_Target) { FfScale_new = FfScale_old + WP_Weight.FfScale.Step; // if(FfScale_new >= WP_Weight.FfScale_Max) FfScale_new = WP_Weight.FfScale_Max; if(FfScale_new <= 0) { // cnt_Ff_LimitLo++; FfScale_new = 0; } } else { FfScale_new = FfScale_old; } FfScaleDiff = FfScale_new - FfScale_Target; if(-WP_Weight.FfScale.Step < FfScaleDiff && FfScaleDiff < WP_Weight.FfScale.Step) { FfScale_new = FfScale_Target; } WP_Weight.FfScale.Temp[side] = FfScale_new; } int count_WeightToggle[L]; void WeightOnOff(int side){ // HIL 모드: OnOffScale 고정, 속도 기반 점진 변화 스킵 if(Debug_HilMode) return; switch(WP_Weight.Ctrl.OnOffStatus[side]){ case ON: if(WP_Weight.Ctrl.OnOffScale[side] < 1.0f) { if(WP_Gym.Speed[L] > -WP_Weight.RefSpdStop && WP_Gym.Speed[R] > -WP_Weight.RefSpdStop) { // 정지 판단 속도 이상으로 내려가지 않는 경우 WP_Weight.Ctrl.OnOffScale[L] = WP_Weight.Ctrl.OnOffScale[L] + WP_Weight.Ctrl.OnOffStep; if(WP_Weight.Ctrl.OnOffScale[L] >= 1.0f) { WP_Weight.Ctrl.OnOffScale[L] = 1.0f; } WP_Weight.Ctrl.OnOffScale[R] = WP_Weight.Ctrl.OnOffScale[R] + WP_Weight.Ctrl.OnOffStep; if(WP_Weight.Ctrl.OnOffScale[R] >= 1.0f) { WP_Weight.Ctrl.OnOffScale[R] = 1.0f; WP_Weight.Ctrl.MotionAutoStatus = STOP; // 앱과 통신하는 변수 업데이트 // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.WeightOnOff_Status = WP_Weight.Ctrl.OnOffStatus[side]; ISR_Deferred.WeightOnOff_Ready = 1; } } } break; case OFF: if(WP_Weight.Ctrl.OnOffScale[side] > 0.0f) { if(WP_Gym.Speed[L] < WP_Weight.RefSpdStop && WP_Gym.Speed[R] < WP_Weight.RefSpdStop) { // 정지 판단 속도 이상으로 올라가지 않는 경우 WP_Weight.Ctrl.OnOffScale[L] = WP_Weight.Ctrl.OnOffScale[L] - WP_Weight.Ctrl.OnOffStep; if(WP_Weight.Ctrl.OnOffScale[L] <= 0.0f) { WP_Weight.Ctrl.OnOffScale[L] = 0.0f; } WP_Weight.Ctrl.OnOffScale[R] = WP_Weight.Ctrl.OnOffScale[R] - WP_Weight.Ctrl.OnOffStep; if(WP_Weight.Ctrl.OnOffScale[R] <= 0.0f) { WP_Weight.Ctrl.OnOffScale[R] = 0.0f; WP_Weight.Ctrl.MotionAutoStatus = STOP; // 앱과 통신하는 변수 업데이트 // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.WeightOnOff_Status = WP_Weight.Ctrl.OnOffStatus[side]; ISR_Deferred.WeightOnOff_Ready = 1; } } } break; } } // Task10ms_1에서 구동중 void WeightBLEtoSetTask(float step) { if(step >= 0.5) step = 0.5; // 무게 올려야 하는 경우 if(WP_Gym.WeightSet[L] < WP_Gym.WeightBLE[L]){ // 유저가 잘 버티고 있다면 if(WP_Gym.Speed[L] > -WP_Weight.RefSpdStop && WP_Gym.Speed[R] > -WP_Weight.RefSpdStop) { // 정지 판단 속도 이상으로 내려가지 않는 경우 WP_Gym.WeightSet[L] = WP_Gym.WeightSet[L] + step; WP_Gym.WeightSet[R] = WP_Gym.WeightSet[R] + step; Return_4x1byte(WEIGHTPLUS, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]); } else { // 유저가 당겨지고 있다면 UART3_Puts("User is not stable\r\n"); } } // 무게 내려야 하는 경우 else if(WP_Gym.WeightSet[L] > WP_Gym.WeightBLE[L]){ // 유저가 안 튀어오르고 있다면 if(WP_Gym.Speed[L] < WP_Weight.RefSpdStop && WP_Gym.Speed[R] < WP_Weight.RefSpdStop) { // 정지 판단 속도 이상으로 올라가지 않는 경우 WP_Gym.WeightSet[L] = WP_Gym.WeightSet[L] - 3.0f*step; // 빠지는 속도는 2에서 3으로 올림 -> 계속 개선 필요(25.05.05) WP_Gym.WeightSet[R] = WP_Gym.WeightSet[R] - 3.0f*step; Return_4x1byte(WEIGHTPLUS, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]); } else { // 유저가 튀어오르고 있다면 UART3_Puts("User is not stable\r\n"); } } // 거의 근접한 경우 그냥 보내버림 if(-step <= WP_Gym.WeightSet[L] - WP_Gym.WeightBLE[L] && WP_Gym.WeightSet[L] - WP_Gym.WeightBLE[L] <= step){ WP_Gym.WeightSet[L] = WP_Gym.WeightBLE[L]; if(-step <= WP_Gym.WeightSet[R] - WP_Gym.WeightBLE[R] && WP_Gym.WeightSet[R] - WP_Gym.WeightBLE[R] <= step){ WP_Gym.WeightSet[R] = WP_Gym.WeightBLE[R]; Task_Stop_10ms_1(); // 현재 A와 kg 구분이 혼재되고 있음 -> 다 뜯어 고쳐야 함!! // 0.25A 단위로 변경하는 현재 기준에서, 4를 곱해서 1 LSB 당 500g 단위로 보내줌 UART3_Puts("Weight: "); UART3_Puts(Float2String(WP_Gym.WeightSet[L],2)); UART3_Puts(" [kg]\r\n"); Return_4x1byte(WEIGHTPLUS, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]); } } // (3/19) Eccen 설정을 현재 무게에 대한 %로 변경했으므로, 서로 상호 업데이트 연동이 필요함 } int cnt_ConvAng2Pos; uint8_t Debug_HilMode = 0; // [v1.0.4] HIL 모드: 0=실측, 1=position 주입 void ConvAng2Pos (void) { // cnt_ConvAng2Pos++; // 실측 모드: 엔코더 → Position/Speed 변환 // 단위 : Position [1mm], PositionFine [0.05mm], PositionFine2 [0.1mm] WP_Gym.Position[L] = -WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; WP_Gym.PositionFine[L] = -20 * WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; if(WP_Gym.PositionFine[L] <= 0) WP_Gym.PositionFine[L] = 0; WP_Gym.PositionFine2[L] = -10 * WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; WP_Gym.Position[R] = -WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; WP_Gym.PositionFine[R] = -20 * WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; if(WP_Gym.PositionFine[R] <= 0) WP_Gym.PositionFine[R] = 0; WP_Gym.PositionFine2[R] = -10 * WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv; WP_Gym.Speed[L] = -WP_Machine.SpoolDiameter *0.5f * (Motor1_Ang.RpmFil *TR_RPM2RAD) * WP_Machine.SensorGearRatioInv; WP_Gym.Speed[R] = -WP_Machine.SpoolDiameter *0.5f * (Motor2_Ang.RpmFil *TR_RPM2RAD) * WP_Machine.SensorGearRatioInv; // 가속도 계산 (10kHz, LPF 적용된 Speed 미분) float dt_inv = 10000.0f; // 1/dt = 1/0.0001 float alpha = 0.02f; // LPF α (fc≈32Hz @10kHz) for (int s = 0; s < 2; s++) { float rawAcc = (WP_Gym.Speed[s] - DevReport_PrevSpd[s]) * dt_inv; DevReport_Accel[s] += alpha * (rawAcc - DevReport_Accel[s]); // 1차 LPF DevReport_PrevSpd[s] = WP_Gym.Speed[s]; } } void ConvAng2Pos_HIL(void) { // HIL 모드: PositionFine은 0xF5 03 핸들러에서 주입됨 (0.05mm) // 나머지 단위만 동기화 WP_Gym.Position[L] = WP_Gym.PositionFine[L] / 20; // 1mm WP_Gym.PositionFine2[L] = WP_Gym.PositionFine[L] / 2; // 0.1mm WP_Gym.Position[R] = WP_Gym.PositionFine[R] / 20; WP_Gym.PositionFine2[R] = WP_Gym.PositionFine[R] / 2; // Speed는 0xF5 03 핸들러에서 직접 주입 — 여기선 건드리지 않음 } void PositionCalibrate (uint8_t side) // 가동범위 세팅 완료 후 '바닥면' 캘리브레이션 기능으로 변경해야 함!! (23.11.22) { switch (side){ case L: // Left delay_msec(WP_UserSetting.TimeCali); WP_Gym.CalibDeg[L] = Motor1_Ang.AngleMechAll; break; case R: // Right delay_msec(WP_UserSetting.TimeCali); WP_Gym.CalibDeg[R] = Motor2_Ang.AngleMechAll; break; case LR: // Both delay_msec(WP_UserSetting.TimeCali); WP_Gym.CalibDeg[L] = Motor1_Ang.AngleMechAll; WP_Gym.CalibDeg[R] = Motor2_Ang.AngleMechAll; break; } } void SetRange(uint8_t side, uint8_t Point) { switch (Point) { case 0: // 저점 세팅 if (WP_Gym.Position[side] > WP_Gym.Region.H_shock + WP_Gym.Region.L_soft) { WP_Gym.Region.RangeLo[side] = WP_Gym.Position[side] + WP_Gym.Region.L_soft; WP_Gym.Region.H_LoSoft[side] = WP_Gym.Region.RangeLo[side] - WP_Gym.Region.L_soft; } else{ UART3_Puts("Err: Too Close to Base (Left)\r\n"); WP_Gym.Region.RangeLo[side] = WP_Gym.Region.H_shock + WP_Gym.Region.L_soft; WP_Gym.Region.H_LoSoft[side] = WP_Gym.Region.H_shock; } break; case 1: // 고점 세팅 if (WP_Gym.Position[side] > WP_Gym.Region.RangeLo[side] + WP_Gym.Region.L_RangeMin) { WP_Gym.Region.RangeHi[side] = WP_Gym.Position[side]; WP_Gym.Region.H_HiSoft[side] = WP_Gym.Region.RangeHi[side] + WP_Gym.Region.L_soft; } else{ UART3_Puts("Err: Too Close to RangeLo (Left)\r\n"); } break; } } //uint32_t SetRangeTemp[3], cnt_SetRangeDigit; void SetRangeDigit(uint8_t side, uint8_t Point, uint16_t Position){ // SetRangeTemp[0] = side; // SetRangeTemp[1] = Point; // SetRangeTemp[2] = Position; switch (Point) { case 0: // 저점 세팅 if (WP_Gym.Region.H_shock + WP_Gym.Region.L_soft < Position && Position < WP_Gym.Region.RangeHi[side] - WP_Gym.Region.L_RangeMin) { WP_Gym.Region.RangeLo[side] = Position; WP_Gym.Region.H_LoSoft[side] = WP_Gym.Region.RangeLo[side] - WP_Gym.Region.L_soft; WP_Gym.Region.RangeError = 0; } else{ UART3_Puts("Err: Too Close to Base or High\r\n"); WP_Gym.Region.RangeError = 2; } break; case 1: // 고점 세팅 if (Position > WP_Gym.Region.RangeLo[side] + WP_Gym.Region.L_RangeMin) { WP_Gym.Region.RangeHi[side] = Position; WP_Gym.Region.H_HiSoft[side] = WP_Gym.Region.RangeHi[side] + WP_Gym.Region.L_soft; WP_Gym.Region.RangeError = 0; } else{ UART3_Puts("Err: Too Close to RangeLo (Left)\r\n"); WP_Gym.Region.RangeError = 2; } break; } // cnt_SetRangeDigit++; } // 좌, 우 독립되지 않고 통합으로 처리되는 임시 코드 (24.12.07) /* 안전을 위해 * On할 경우는 양쪽 모두 모션을 취해야 가능 * Off할 경우는 한쪽만 모션을 취해도 가능 */ int Status_MotionAutoWeight = 0; // 0: None, -1: temporary to Off, +1: temporary to On float Spd_PullAutoOn = 520; float Spd_DropAutoOff = -720; float Spd_StopAutoOff = 35; int Cnt_MotionAutoWeight; int Time_PullAutoOn = 1000; int Time_DropAutoOff = 1500; int Time_StopAutoOff = 50000; int Temp_OnOffStatus = 0; // 테스트용 대체 OnOffStatus int check_MotionAutoWeight; void MotionAutoWeight (void) { if(Debug_HilMode) return; // HIL: 자동 ON/OFF 비활성 check_MotionAutoWeight++; // OnOffStatus와 동기화: 수동 설정에 따른 변경을 반영 if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) { // Status_MotionAutoWeight = 1; Temp_OnOffStatus = ON; } else if(WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) { // Status_MotionAutoWeight = 0; Temp_OnOffStatus = OFF; } // 속도 분석을 통해 상황 판정 // 1. FastPullOn : 양쪽 다 빠르게 당기고 있을 경우, if(WP_Weight.Ctrl.MotionAutoActive == 1 && WP_Gym.Speed[L] > Spd_PullAutoOn && WP_Gym.Speed[R] > Spd_PullAutoOn) { Status_MotionAutoWeight = 1; // 순시적 AutoOn 조건 부합 Cnt_MotionAutoWeight++; // 충분한 시간동안 유지되면 발동 -> 동작이 안정화된 경우 발동하도록 추가 단계 필요!! if(Cnt_MotionAutoWeight > Time_PullAutoOn) { if(WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) { Temp_OnOffStatus = ON; WP_Weight.Ctrl.OnOffStatus[L] = ON; WP_Weight.Ctrl.OnOffStatus[R] = ON; WP_Weight.Ctrl.MotionAutoStatus = UP; // 앱에 알려주기 위함 // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.MotionAuto_StatusL = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_StatusR = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_Ready = 1; WP_LEDCtrl.MotionAutoWeight = 3; // 이 숫자 만큼 깜빡임 } } } // 2A. DropRelease : 양쪽 중 하나라도 빠르게 내려지고 있을 경우, else if(WP_Gym.Speed[L] < Spd_DropAutoOff || WP_Gym.Speed[R] < Spd_DropAutoOff) { Status_MotionAutoWeight = -1; // 순시적 AutoOff 조건 부합 Cnt_MotionAutoWeight--; // 충분한 시간동안 유지되면 발동 if(Cnt_MotionAutoWeight < -Time_DropAutoOff) { // 디버깅 시 구분감을 위해 부호를 반대로 처리함 if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) { Temp_OnOffStatus = OFF; WP_Weight.Ctrl.OnOffStatus[L] = OFF; WP_Weight.Ctrl.OnOffStatus[R] = OFF; WP_Weight.Ctrl.MotionAutoStatus = DOWN; // 앱에 알려주기 위함 (나중엔 좌우 구분할 예정) // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.MotionAuto_StatusL = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_StatusR = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_Ready = 1; WP_LEDCtrl.MotionAutoWeight = -3; // 이 숫자 만큼 깜빡임 } } // 2B. AutoRest : 양쪽 다 충분히 정지할 경우, } else if ((WP_Gym.Speed[L] < Spd_StopAutoOff && WP_Gym.Speed[L] > -Spd_StopAutoOff ) && (WP_Gym.Speed[R] < Spd_StopAutoOff && WP_Gym.Speed[R] > -Spd_StopAutoOff)) { if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) { Status_MotionAutoWeight = -1; // 순시적 AutoOff 조건 부합 Cnt_MotionAutoWeight--; // 충분한 시간동안 유지되면 발동 if(Cnt_MotionAutoWeight < -Time_StopAutoOff) { // 디버깅 시 구분감을 위해 부호를 반대로 처리함 Temp_OnOffStatus = OFF; WP_Weight.Ctrl.OnOffStatus[L] = OFF; WP_Weight.Ctrl.OnOffStatus[R] = OFF; WP_Weight.Ctrl.MotionAutoStatus = DOWN; // 앱에 알려주기 위함 // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.MotionAuto_StatusL = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_StatusR = WP_Weight.Ctrl.MotionAutoStatus; ISR_Deferred.MotionAuto_Ready = 1; WP_LEDCtrl.MotionAutoWeight = -3; // 이 숫자 만큼 깜빡임 Cnt_MotionAutoWeight = 0; // 초기화 } } else if (WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) { Cnt_MotionAutoWeight = 0; } } else { // 2B가 생기면서 이 구간의 역할이 꼬여서 파악 필요 Status_MotionAutoWeight = 0; Cnt_MotionAutoWeight = 0; } } // WeightBLEtoSetTask() 에다가 집어 넣으면 될 듯? (3/26) // WeightController에 있는 게 맞다! (5/5) void ClampEccWeight (uint8_t side) { if(WP_Gym.F_EccSet > 0.5f * WP_Gym.WeightSet[side]) { WP_Gym.F_EccSet = WP_Gym.WeightSet[side] * 0.5f; // 실제 적용되는 변수 // WP_Gym.F_EccSet_Temp[side] = WP_Gym.F_EccSet; // 프로토콜용 임시 변수 -> 향후 이렇게 좌우 구분해야 함 WP_Gym.F_EccSet_Temp[L] = WP_Gym.F_EccSet; // 구조가 꼬여서 각각 바꿔줘야 함.. WP_Gym.F_EccSet_Temp[R] = WP_Gym.F_EccSet; // [v1.0.2] ISR에서 직접 전송 → 플래그로 지연 ISR_Deferred.EccClamp_L = WP_Gym.F_EccSet_Temp[L] * 2; ISR_Deferred.EccClamp_R = WP_Gym.F_EccSet_Temp[R] * 2; ISR_Deferred.EccClamp_Err = WP_Gym.F_Ecc_ErrorCode; ISR_Deferred.EccClamp_Ready = 1; } } /* Init_Encoder Code begin */ /* Init_Encoder_v1 */ //// 강제 회전 조건 (Icmd.q) //float current_sequence[] = {-0.8f, 0.8f, -1.2f, 1.2f, -1.5f, 1.5f}; //float isRotating_spd = 2.0f; //const uint32_t MAX_STEP = sizeof(current_sequence)/sizeof(current_sequence[0]); // //void Init_Encoder(void) { //// 여기서 건드린 모터 제어 관련 변수는 모두 원복 해야 함 !!! // // uint32_t currentTime = Get_Time(); // // // 에러 상태에서는 아예 함수 진입 중단 -> 일단 둘 중 하나라도 에러면 무조건 동작 중지 처리. 나중엔 하나라도 쓸 수 있게..? // if (WP_EncInit.Status[L] == Error || WP_EncInit.Status[R] == Error) { // WP_Ctrl.ActiveMotor = Drv_NoMotor; // return; // } // // 양쪽 모두 초기화 성공 시 (초기화 여부는 외부 HAL_GPIO_EXTI_Callback에서 업데이트함) // if (WP_EncInit.Status[L] == Done && WP_EncInit.Status[R] == Done) { // UART3_Puts("Encoder Init Success\r\n"); // // 모터 제어 관련 변수 한번 더 초기화 (어차피 모터 각각 처리할 때 이미 하긴 함) // Motor1.Cmd.XbyF_Freq = Motor2.Cmd.XbyF_Freq = 0.0f; // Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; // // WP_EncInit.Step[L] = WP_EncInit.Step[R] = 0; // Drive_Status = Calibration; // // return; // } // // // 첫 진입 시 초기화 : 양측 다 스텝1 개시 // if (WP_EncInit.Step[L] == 0 && WP_EncInit.Step[R] == 0) { // WP_Ctrl.Mode = Inv_IbyFCtrl; // Debug_DriveMotor = Drv_BothMotor; // WP_Ctrl.ActiveMotor = Drv_BothMotor; // Motor1.Cmd.XbyF_Freq = Motor2.Cmd.XbyF_Freq = 1.0f; // WP_EncInit.StartTime[L] = WP_EncInit.StartTime[R] = currentTime; // WP_EncInit.Step[L] = WP_EncInit.Step[R] = 1; // Motor1.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1]; // Motor2.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1]; // UART3_Puts("Encoder Init Started\r\n"); // return; // } else { // // 좌측 모터 관리 // if (WP_EncInit.Status[L] == NotDone) { // WP_EncInit.StepTime[L] = currentTime - WP_EncInit.StartTime[L]; // // // 현재 회전 여부 // // 속도가 기준치를 넘는다면 정상 회전 // if (Abs(Motor1_Ang.RpmFil) > isRotating_spd) { // // 현상 유지. 외부에서 초기화 신호가 올 때까지 대기 // // HAL_GPIO_EXTI_Callback에서 WP_EncInit.Status[ ] = Done으로 설정될 예정 // // // 회전하는데도 스텝 타임이 2초를 초과한다면 : 초기화가 안 되는 에러로 판단. 에러 플래그 // if (WP_EncInit.StepTime[L] > ZPULSE_WAIT_TIME) { // WP_EncInit.Status[L] = Error; // Motor1.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.XbyF_Freq = 0.0f; // } // } // // 회전하지 않는 상태 // else if (WP_EncInit.StepTime[L] > ROTATION_WAIT_TIME) { // // 스텝이 마지막 전류시퀀스라면 에러 플래그 // if (WP_EncInit.Step[L] > MAX_STEP) { // UART3_Puts("좌측 초기화 실패 \r\n"); // WP_EncInit.Status[L] = Error; // Motor1.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.XbyF_Freq = 0.0f; // } // // 스텝 타임이 0.5초를 초과한다면 다음 스텝으로 넘어가서 세팅 // else { // UART3_Puts("좌측 움직이지 않음 \r\n"); // WP_EncInit.Step[L]++; // WP_EncInit.StartTime[L] = currentTime; // Motor1.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1]; // } // } // } // // Done 또는 Error에서는 아무것도 하지 않음. 안전을 위해 구동 중지 초기화 더블 체크 // else { // Motor1.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.XbyF_Freq = 0.0f; // } // // // 우측 모터 관리 // if (WP_EncInit.Status[R] == NotDone) { // WP_EncInit.StepTime[R] = currentTime - WP_EncInit.StartTime[R]; // // // 현재 회전 여부 // // 속도가 기준치를 넘는다면 정상 회전 // if (Abs(Motor2_Ang.RpmFil) > isRotating_spd) { // // 현상 유지. 외부에서 초기화 신호가 올 때까지 대기 // // HAL_GPIO_EXTI_Callback에서 WP_EncInit.Status[ ] = Done으로 설정될 예정 // // // 회전하는데도 스텝 타임이 2초를 초과한다면 : 초기화가 안 되는 에러로 판단. 에러 플래그 // if (WP_EncInit.StepTime[R] > ZPULSE_WAIT_TIME) { // WP_EncInit.Status[R] = Error; // Motor2.Cmd.Icmd.q = 0.0f; // Motor2.Cmd.XbyF_Freq = 0.0f; // } // } // // 회전하지 않는 상태 // else if (WP_EncInit.StepTime[R] > ROTATION_WAIT_TIME) { // // 스텝이 마지막 전류시퀀스라면 에러 플래그 // if (WP_EncInit.Step[R] > MAX_STEP) { // UART3_Puts("우측 초기화 실패 \r\n"); // WP_EncInit.Status[R] = Error; // Motor2.Cmd.Icmd.q = 0.0f; // Motor2.Cmd.XbyF_Freq = 0.0f; // } // // 스텝 타임이 0.5초를 초과한다면 다음 스텝으로 넘어가서 세팅 // else { // UART3_Puts("우측 움직이지 않음 \r\n"); // WP_EncInit.Step[R]++; // WP_EncInit.StartTime[R] = currentTime; // Motor2.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[R] - 1]; // } // } // } // // Done 또는 Error에서는 아무것도 하지 않음. 안전을 위해 구동 중지 초기화 더블 체크 // else { // Motor2.Cmd.Icmd.q = 0.0f; // Motor2.Cmd.XbyF_Freq = 0.0f; // } // } //} /* Init_Encoder_v2 */ //void Init_Encoder_v2(void) { // uint32_t currentTime = Get_Time(); // // // 에러 상태에서는 함수 진입 중단 // if (WP_EncInit.Status[L] == Error || WP_EncInit.Status[R] == Error) { // WP_Ctrl.ActiveMotor = Drv_NoMotor; // return; // } // // // 양쪽 모두 초기화 성공 시 // if (WP_EncInit.Status[L] == Done && WP_EncInit.Status[R] == Done) { // UART3_Puts("Encoder Init Success\r\n"); // // // AngleElecOffset 원래값으로 복구 // Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; // Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset; // // // 모터 제어 변수 초기화 // Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; // // WP_EncInit.Step[L] = WP_EncInit.Step[R] = 0; // Drive_Status = Calibration; // return; // } // // // 첫 진입 시 초기화 // if (WP_EncInit.Step[L] == 0 && WP_EncInit.Step[R] == 0) { // WP_Ctrl.Mode = Inv_TorqueCtrl; // TorqueCtrl 사용 // Debug_DriveMotor = Drv_BothMotor; // WP_Ctrl.ActiveMotor = Drv_BothMotor; // // // 초기 전류 설정 (-0.8A부터 시작) // Motor1.Cmd.Icmd.q = -0.8f; // Motor2.Cmd.Icmd.q = -0.8f; // Motor1.Cmd.Icmd.d = 0.0f; // Motor2.Cmd.Icmd.d = 0.0f; // // // 시작 시간 기록 // WP_EncInit.StartTime[L] = WP_EncInit.StartTime[R] = currentTime; // // // 초기 AngleElecOffset 설정 (원래값 + 첫 번째 오프셋) // Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + 0.0f; // Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + 0.0f; // // WP_EncInit.Step[L] = WP_EncInit.Step[R] = 1; // // UART3_Puts("Encoder Init Started (TorqueCtrl)\r\n"); // return; // } // else { // // AngleSequence 배열 정의 // const float AngleSequence[5] = {0.0f, 30.0f, 60.0f, 90.0f, 120.0f}; // const float CurrentSequence[4] = {-0.8f, 0.8f, -1.2f, 1.2f}; // // // 좌우 독립 진행을 위한 지역변수 // static uint8_t CurrentLevel_L = 0, CurrentLevel_R = 0; // 0:-0.8A, 1:0.8A, 2:-1.2A, 3:1.2A // static uint8_t AngleStep_L = 0, AngleStep_R = 0; // // // 좌측 모터 관리 // if (WP_EncInit.Status[L] == NotDone) { // WP_EncInit.StepTime[L] = currentTime - WP_EncInit.StartTime[L]; // // // 회전 검출 (속도가 기준치를 넘으면 성공) // if (Abs(Motor1_Ang.RpmFil) > isRotating_spd) { // // 회전 중 - Z펄스 대기 // if (WP_EncInit.StepTime[L] > ZPULSE_WAIT_TIME) { // // Z펄스가 오지 않으면 다음 각도로 // UART3_printf("좌측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n", // AngleStep_L, CurrentLevel_L); // AngleStep_L++; // WP_EncInit.StartTime[L] = currentTime; // // if (AngleStep_L >= 5) { // // 모든 각도 시도 완료 // AngleStep_L = 0; // CurrentLevel_L++; // // if (CurrentLevel_L >= 4) { // // 모든 전류 레벨 시도 완료 - 에러 // UART3_Puts("좌측 초기화 실패 (모든 시도 완료)\r\n"); // WP_EncInit.Status[L] = Error; // Motor1.Cmd.Icmd.q = 0.0f; // return; // } else { // UART3_printf("좌측 전류 변경: %s A\r\n", // Float2String(CurrentSequence[CurrentLevel_L], 1)); // } // } // // // 새로운 각도와 전류 설정 // Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[AngleStep_L]; // Motor1.Cmd.Icmd.q = CurrentSequence[CurrentLevel_L]; // } // } // // 회전하지 않는 상태 // else if (WP_EncInit.StepTime[L] > ROTATION_WAIT_TIME) { // UART3_printf("좌측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n", // AngleStep_L, CurrentLevel_L); // // AngleStep_L++; // WP_EncInit.StartTime[L] = currentTime; // // if (AngleStep_L >= 5) { // // 모든 각도 시도 완료 // AngleStep_L = 0; // CurrentLevel_L++; // // if (CurrentLevel_L >= 4) { // // 모든 전류 레벨 시도 완료 - 에러 // UART3_Puts("좌측 초기화 실패 (모든 시도 완료)\r\n"); // WP_EncInit.Status[L] = Error; // Motor1.Cmd.Icmd.q = 0.0f; // return; // } else { // UART3_printf("좌측 전류 변경: %s A\r\n", // Float2String(CurrentSequence[CurrentLevel_L], 1)); // } // } // // // 새로운 각도와 전류 설정 // Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[AngleStep_L]; // Motor1.Cmd.Icmd.q = CurrentSequence[CurrentLevel_L]; // } // } // // Done 또는 Error에서는 모터 정지 // else { // Motor1.Cmd.Icmd.q = 0.0f; // Motor1.Cmd.Icmd.d = 0.0f; // } // // // 우측 모터 관리 (좌측과 독립적으로 진행) // if (WP_EncInit.Status[R] == NotDone) { // WP_EncInit.StepTime[R] = currentTime - WP_EncInit.StartTime[R]; // // // 회전 검출 // if (Abs(Motor2_Ang.RpmFil) > isRotating_spd) { // // 회전 중 - Z펄스 대기 // if (WP_EncInit.StepTime[R] > ZPULSE_WAIT_TIME) { // UART3_printf("우측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n", // AngleStep_R, CurrentLevel_R); // AngleStep_R++; // WP_EncInit.StartTime[R] = currentTime; // // if (AngleStep_R >= 5) { // AngleStep_R = 0; // CurrentLevel_R++; // // if (CurrentLevel_R >= 4) { // UART3_Puts("우측 초기화 실패 (모든 시도 완료)\r\n"); // WP_EncInit.Status[R] = Error; // Motor2.Cmd.Icmd.q = 0.0f; // return; // } else { // UART3_printf("우측 전류 변경: %s A\r\n", // Float2String(CurrentSequence[CurrentLevel_R], 1)); // } // } // // Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[AngleStep_R]; // Motor2.Cmd.Icmd.q = CurrentSequence[CurrentLevel_R]; // } // } // // 회전하지 않는 상태 // else if (WP_EncInit.StepTime[R] > ROTATION_WAIT_TIME) { // UART3_printf("우측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n", // AngleStep_R, CurrentLevel_R); // // AngleStep_R++; // WP_EncInit.StartTime[R] = currentTime; // // if (AngleStep_R >= 5) { // AngleStep_R = 0; // CurrentLevel_R++; // // if (CurrentLevel_R >= 4) { // UART3_Puts("우측 초기화 실패 (모든 시도 완료)\r\n"); // WP_EncInit.Status[R] = Error; // Motor2.Cmd.Icmd.q = 0.0f; // return; // } else { // UART3_printf("우측 전류 변경: %s A\r\n", // Float2String(CurrentSequence[CurrentLevel_R], 1)); // } // } // // Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[AngleStep_R]; // Motor2.Cmd.Icmd.q = CurrentSequence[CurrentLevel_R]; // } // } // // Done 또는 Error에서는 모터 정지 // else { // Motor2.Cmd.Icmd.q = 0.0f; // Motor2.Cmd.Icmd.d = 0.0f; // } // } //} /* Init_Encoder_v3 */ void Init_Encoder_v3(void) { const float RefSpd_Moving = 2.0f; // 회전 감지 기준 속도 // 시간 설정 (유지보수 용이성) const uint32_t ROTATION_WAIT_TIME = 1000; // 회전 대기 시간 const uint32_t ZPULSE_WAIT_TIME = 5000; // Z펄스 대기 시간 // AngleSequence 배열과 CurrentSequence 배열을 함수 전체에서 사용 //const float AngleSequence[10] = {0.0f, 30.0f, 60.0f, 90.0f, 120.0f, 150.0f, 180.0f, 210.0f, 240.0f, 270.0f}; // 옵션 1: 대칭 우선 //const float AngleSequence[10] = {0.0f, 180.0f, 90.0f, 270.0f, 45.0f, 225.0f, 135.0f, 315.0f, 30.0f, 210.0f}; // 옵션 2: 90도 간격 우선 (기계적으로 가장 다른 위치) const float AngleSequence[10] = {0.0f, 90.0f, 180.0f, 270.0f, 45.0f, 135.0f, 225.0f, 315.0f, 30.0f, 60.0f}; const float CurrentSequence[2] = {0.75f, 1.0f}; uint8_t num_Ang = sizeof(AngleSequence) / sizeof(AngleSequence[0]); uint8_t num_Curr = sizeof(CurrentSequence) / sizeof(CurrentSequence[0]); uint32_t currentTime = Get_Time(); // 에러 상태에서는 함수 진입 중단 if (WP_EncInit.Status[L] == Error || WP_EncInit.Status[R] == Error) { // WP_Ctrl.Mode = Inv_Off; // Debug_DriveMotor = Drv_NoMotor; // WP_Ctrl.ActiveMotor = Drv_NoMotor; WP_Ctrl.Mode = Inv_Off; WP_Ctrl.ActiveMotor = Drv_NoMotor; Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; Debug_DriveMotor = Drv_NoMotor; WP_EncInit.Flag = Error; return; } // 양쪽 모두 초기화 성공 시 if (WP_EncInit.Status[L] == Done && WP_EncInit.Status[R] == Done) { UART3_Puts("Encoder Init Success\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Flag = Done; // AngleElecOffset 원래값으로 복구 Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset; // 모터 제어 변수 초기화 Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; WP_Ctrl.Mode = Inv_Off; Debug_DriveMotor = Drv_NoMotor; WP_Ctrl.ActiveMotor = Drv_NoMotor; WP_EncInit.Step[L] = WP_EncInit.Step[R] = 0; // NOTE: 구조체에서 Step[2] 제거됨 - AngleStep[2]로 대체 // WP_EncInit.AngleStep[L] = WP_EncInit.AngleStep[R] = 0; // 구조체 초기화 시 자동으로 0 // Drive_Status = Calibration; WP_ForcedCali.Flag = Done; Drive_Status = RunGym; Debug_DriveMotor = Drv_BothMotor; return; } // 첫 진입 시 초기화 if (WP_EncInit.Step[L] == 0 && WP_EncInit.Step[R] == 0) { // NOTE: 구조체에서 Step[2] 제거 예정 - AngleStep 사용 고려 WP_Ctrl.Mode = Inv_TorqueCtrl; // TorqueCtrl 사용 Debug_DriveMotor = Drv_BothMotor; WP_Ctrl.ActiveMotor = Drv_BothMotor; // 배열을 이용한 초기 설정 Motor1.Cmd.Icmd.q = CurrentSequence[0]; // 0.8A Motor2.Cmd.Icmd.q = CurrentSequence[0]; // 0.8A // 시작 시간 기록 WP_EncInit.StartTime[L] = WP_EncInit.StartTime[R] = currentTime; // 배열을 이용한 초기 AngleElecOffset 설정 Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[0]; // +0도 Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[0]; // +0도 // 안전 회전량 체크를 위한 시작 각도 저장 WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; WP_EncInit.Step[L] = WP_EncInit.Step[R] = 1; // NOTE: 구조체에서 Step[2] 제거 예정 UART3_Puts("Encoder Init Started (TorqueCtrl)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) return; } // 재진입 시 else { // 좌우 독립 진행을 위한 변수들 (WP_EncInit 구조체 사용) // 좌측 모터 관리 if (WP_EncInit.Status[L] == NotDone) { WP_EncInit.StepTime[L] = currentTime - WP_EncInit.StartTime[L]; // 역방향 회전 시 안전 회전량 체크 (360도 제한) if (Motor1_Ang.RpmFil < -RefSpd_Moving) { float rotationAmount = Abs(Motor1_Ang.AngleElec - WP_EncInit.StartAngle[L]); if (rotationAmount > 180.0f) rotationAmount = 360.0f - rotationAmount; // 최단 경로로 계산 if (rotationAmount > 360.0f) { Debug_UART3_printf("좌측 풀림 제한: 360도 초과 회전 감지 - 다음 시도\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[L]++; WP_EncInit.StartTime[L] = currentTime; WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[L] >= num_Ang) { WP_EncInit.AngleStep[L] = 0; WP_EncInit.CurrentLevel[L]++; if (WP_EncInit.CurrentLevel[L] >= num_Curr) { Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 회전 제한)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[L] = Error; Motor1.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("좌측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[L]], 1)); } } Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[L]]; Motor1.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[L]]; } } // 회전 감지 및 Z펄스 대기 / 타임아웃 체크 if (Abs(Motor1_Ang.RpmFil) > RefSpd_Moving) { // 회전 중 - Z펄스 대기 if (WP_EncInit.StepTime[L] > ZPULSE_WAIT_TIME) { // Z펄스가 오지 않으면 다음 각도로 Debug_UART3_printf("좌측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[L], WP_EncInit.CurrentLevel[L]); WP_EncInit.AngleStep[L]++; WP_EncInit.StartTime[L] = currentTime; WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[L] >= num_Ang) { // 모든 각도 시도 완료 WP_EncInit.AngleStep[L] = 0; WP_EncInit.CurrentLevel[L]++; if (WP_EncInit.CurrentLevel[L] >= num_Curr) { // 모든 전류 레벨 시도 완료 - 에러 Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 타임아웃)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[L] = Error; Motor1.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("좌측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[L]], 1)); } } // 새로운 각도와 전류 설정 Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[L]]; Motor1.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[L]]; } } else if (WP_EncInit.StepTime[L] > ROTATION_WAIT_TIME) { // 회전 안 됨 - 다음 각도로 Debug_UART3_printf("좌측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[L], WP_EncInit.CurrentLevel[L]); WP_EncInit.AngleStep[L]++; WP_EncInit.StartTime[L] = currentTime; WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[L] >= num_Ang) { // 모든 각도 시도 완료 WP_EncInit.AngleStep[L] = 0; WP_EncInit.CurrentLevel[L]++; if (WP_EncInit.CurrentLevel[L] >= num_Curr) { // 모든 전류 레벨 시도 완료 - 에러 Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 회전 없음)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[L] = Error; Motor1.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("좌측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[L]], 1)); } } // 새로운 각도와 전류 설정 Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[L]]; Motor1.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[L]]; } } // Done 또는 Error에서는 모터 정지 else { Motor1.Cmd.Icmd.q = 0.0f; Motor1.Cmd.Icmd.d = 0.0f; } // 우측 모터 관리 (좌측과 독립적으로 진행) if (WP_EncInit.Status[R] == NotDone) { WP_EncInit.StepTime[R] = currentTime - WP_EncInit.StartTime[R]; // 안전 회전량 체크 (360도 제한) if (Motor2_Ang.RpmFil < -RefSpd_Moving) { float rotationAmount = Abs(Motor2_Ang.AngleElec - WP_EncInit.StartAngle[R]); if (rotationAmount > 180.0f) rotationAmount = 360.0f - rotationAmount; // 최단 경로로 계산 if (rotationAmount > 360.0f) { Debug_UART3_printf("우측 안전 제한: 360도 초과 회전 감지 - 다음 시도\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[R]++; WP_EncInit.StartTime[R] = currentTime; WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[R] >= num_Ang) { WP_EncInit.AngleStep[R] = 0; WP_EncInit.CurrentLevel[R]++; if (WP_EncInit.CurrentLevel[R] >= num_Curr) { Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 회전 제한)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[R] = Error; Motor2.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("우측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[R]], 1)); } } Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[R]]; Motor2.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[R]]; } } // 회전 감지 및 Z펄스 대기 / 타임아웃 체크 if (Abs(Motor2_Ang.RpmFil) > RefSpd_Moving) { // 회전 중 - Z펄스 대기 if (WP_EncInit.StepTime[R] > ZPULSE_WAIT_TIME) { // Z펄스가 오지 않으면 다음 각도로 Debug_UART3_printf("우측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[R], WP_EncInit.CurrentLevel[R]); WP_EncInit.AngleStep[R]++; WP_EncInit.StartTime[R] = currentTime; WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[R] >= num_Ang) { // 모든 각도 시도 완료 WP_EncInit.AngleStep[R] = 0; WP_EncInit.CurrentLevel[R]++; if (WP_EncInit.CurrentLevel[R] >= num_Curr) { // 모든 전류 레벨 시도 완료 - 에러 Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 타임아웃)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[R] = Error; Motor2.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("우측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[R]], 1)); } } // 새로운 각도와 전류 설정 Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[R]]; Motor2.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[R]]; } } else if (WP_EncInit.StepTime[R] > ROTATION_WAIT_TIME) { // 회전 안 됨 - 다음 각도로 Debug_UART3_printf("우측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.AngleStep[R], WP_EncInit.CurrentLevel[R]); WP_EncInit.AngleStep[R]++; WP_EncInit.StartTime[R] = currentTime; WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; // 새 시작 각도 저장 if (WP_EncInit.AngleStep[R] >= num_Ang) { // 모든 각도 시도 완료 WP_EncInit.AngleStep[R] = 0; WP_EncInit.CurrentLevel[R]++; if (WP_EncInit.CurrentLevel[R] >= num_Curr) { // 모든 전류 레벨 시도 완료 - 에러 Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 회전 없음)\r\n"); // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) WP_EncInit.Status[R] = Error; Motor2.Cmd.Icmd.q = 0.0f; return; // SAFETY: 한쪽 모터라도 에러 시 전체 중단 (안전상 필요) } else { Debug_UART3_printf("우측 전류 변경: %s A\r\n", // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험) Float2String(CurrentSequence[WP_EncInit.CurrentLevel[R]], 1)); } } // 새로운 각도와 전류 설정 Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[R]]; Motor2.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[R]]; } } // Done 또는 Error에서는 모터 정지 else { Motor2.Cmd.Icmd.q = 0.0f; Motor2.Cmd.Icmd.d = 0.0f; } } } /* ForcedCalibration */ //int debug_FrcCali = 0; //int cnt_FrcCali = 0; int cnt_LED_EncInit, cnt_LED_EncInit_Err, cnt_LED_Calib, cnt_LED_Calib_Err; int Active_ForcedCalib = 0; int ReStart_ForcedCalib = 0; void ForcedCalibration(void) { const float RefSpd_Moving = 2.0f; // 회전 감지 기준 속도 const float ForcedCaliCurrents[4] = {0.4f, 0.5f, 0.9f, 1.1f}; // 수행시간 단축을 위해 변수 없애보기 (250705 오후) //const uint32_t FORCED_CALI_TIMEOUT = 20000; // 20초 //const uint32_t STOP_WAIT_TIME = 750; // 1초 정지 대기 // const float MAX_ROTATION_TURNS = 6480.0f; // 18바퀴 const float MAX_POSITION_MM = 3500.0f; // 3.5m const float MAX_REVERSE_ROTATION = 180.0f; // 역방향 회전 제한 (도) const float MAX_WINDING_SPEED = 250.0f; // 최대 감는 속도 (RPM) // const uint32_t REVERSE_RETRY_WAIT = 5000; // 5초 역방향 재시도 대기 // const uint8_t MAX_REVERSE_RETRY = 3; // 최대 3회 재시도 uint32_t currentTime = Get_Time(); // 에러 시 중단 if (WP_ForcedCali.Status[L] == Error || WP_ForcedCali.Status[R] == Error) { WP_Ctrl.Mode = Inv_Off; WP_Ctrl.ActiveMotor = Drv_NoMotor; Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; Debug_DriveMotor = Drv_NoMotor; WP_ForcedCali.Flag = Error; return; } // 양쪽 완료 시 if (WP_ForcedCali.Status[L] == Done && WP_ForcedCali.Status[R] == Done) { if(WP_ForcedCali.Flag != Done) { Debug_UART3_printf("Forced Calibration Success\r\n"); WP_ForcedCali.Flag = Done; } // 모터 제어 변수 초기화 Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = 0.0f; Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; WP_Ctrl.Mode = Inv_Off; Debug_DriveMotor = Drv_NoMotor; // 이게 맞나? WP_Ctrl.ActiveMotor = Drv_NoMotor; // 성공 시 웰컴라이트 띄워주고 플래그 받고 나서 RunGym으로 업데이트하자! if(cnt_LED_Welcome > 0){ Drive_Status = RunGym; Debug_DriveMotor = Drv_BothMotor; } return; } // 첫 진입 시 초기화 if (WP_ForcedCali.Step[L] == 0 && WP_ForcedCali.Step[R] == 0) { WP_Ctrl.Mode = Inv_TorqueCtrl; Debug_DriveMotor = Drv_BothMotor; WP_Ctrl.ActiveMotor = Drv_BothMotor; WP_ForcedCali.StartTime[L] = WP_ForcedCali.StartTime[R] = currentTime; WP_ForcedCali.Step[L] = WP_ForcedCali.Step[R] = 1; WP_ForcedCali.CurrentLevel[L] = WP_ForcedCali.CurrentLevel[R] = 2; // 0.8A에서 시작 WP_ForcedCali.StartAngle[L] = Motor1_Ang.AngleMechAll; WP_ForcedCali.StartAngle[R] = Motor2_Ang.AngleMechAll; WP_ForcedCali.StartPosition[L] = WP_Gym.Position[L]; WP_ForcedCali.StartPosition[R] = WP_Gym.Position[R]; Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = ForcedCaliCurrents[2]; // 0.8A Motor1.Cmd.Icmd.d = Motor2.Cmd.Icmd.d = 0.0f; Debug_UART3_printf("Forced Calibration Started (TorqueCtrl)\r\n"); return; } // 좌우 모터 처리 루프 for (int motor = L; motor <= R; motor++) { if (WP_ForcedCali.Status[motor] != NotDone) continue; float rpm = (motor == L) ? Motor1_Ang.RpmFil : Motor2_Ang.RpmFil; float angle = (motor == L) ? Motor1_Ang.AngleMechAll : Motor2_Ang.AngleMechAll; float position = (motor == L) ? WP_Gym.Position[L] : WP_Gym.Position[R]; // 과속 감는 속도 체크 (전류 감소로 속도 제어) if (rpm > MAX_WINDING_SPEED) { if (WP_ForcedCali.CurrentLevel[motor] > 0) { WP_ForcedCali.CurrentLevel[motor]--; // 더 낮은 전류로 float newCurrent = ForcedCaliCurrents[WP_ForcedCali.CurrentLevel[motor]]; Debug_UART3_printf("%s 과속 감지 - 전류 감소: %s A\r\n", (motor == L) ? "좌측" : "우측", Float2String(newCurrent, 1)); if (motor == L) Motor1.Cmd.Icmd.q = newCurrent; else Motor2.Cmd.Icmd.q = newCurrent; } // 최소 전류(0.4A)에서도 과속이면 그냥 유지 } // 역방향 180도 초과 회전 체크 /* 재시도 없는 버전 보존 */ if (rpm < -RefSpd_Moving) { float reverseRotation = WP_ForcedCali.StartAngle[motor] - angle; // 역방향은 감소 if (reverseRotation > MAX_REVERSE_ROTATION) { Debug_UART3_printf("%s 역방향 180도 초과 회전 감지 - 에러\r\n", (motor == L) ? "좌측" : "우측"); WP_ForcedCali.Status[motor] = Error; if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; return; } } // 타임아웃 체크 if (currentTime - WP_ForcedCali.StartTime[motor] > 20000) { // FORCED_CALI_TIMEOUT Debug_UART3_printf("%s 강제 캘리브레이션 타임아웃 (20초 초과)\r\n", (motor == L) ? "좌측" : "우측"); WP_ForcedCali.Status[motor] = Error; if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; return; } /* // 과회전 체크 (주석처리) if ((angle - WP_ForcedCali.StartAngle[motor]) > MAX_ROTATION_TURNS) { UART3_printf("%s 과도한 회전량 감지 (18바퀴 초과)\r\n", (motor == L) ? "좌측" : "우측"); WP_ForcedCali.Status[motor] = Error; if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; return; } */ // 과이동 체크 if ((position - WP_ForcedCali.StartPosition[motor]) > MAX_POSITION_MM) { Debug_UART3_printf("%s 과도한 이동거리 감지 (3.5m 초과)\r\n", (motor == L) ? "좌측" : "우측"); WP_ForcedCali.Status[motor] = Error; if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; return; } // 회전/정지 상태 처리 uint8_t isRotating = (Abs(rpm) > RefSpd_Moving); if (WP_ForcedCali.WasRotating[motor] && !isRotating) { // 회전→정지 전환 WP_ForcedCali.StopTime[motor] = currentTime; } if (!isRotating && (currentTime - WP_ForcedCali.StopTime[motor]) > 750) { // STOP_WAIT_TIME // 1초 이상 정지 - 회전 안 되면 전류 상승, 회전 후 정지면 완료 if (WP_ForcedCali.WasRotating[motor]) { // 회전 후 정지 - 완료 Debug_UART3_printf("%s 강제 캘리브레이션 완료\r\n", (motor == L) ? "좌측" : "우측"); if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; Debug_UART3_printf("PositionCalibrate(%d) 호출 전\r\n", motor); PositionCalibrate(motor); // L(0) 또는 R(1) 직접 전달 Debug_UART3_printf("PositionCalibrate(%d) 호출 후\r\n", motor); WP_ForcedCali.Status[motor] = Done; if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; } else { // 회전 안 됨 - 전류 상승 if (WP_ForcedCali.CurrentLevel[motor] < 3) { WP_ForcedCali.CurrentLevel[motor]++; // 더 높은 전류로 float newCurrent = ForcedCaliCurrents[WP_ForcedCali.CurrentLevel[motor]]; Debug_UART3_printf("%s 회전 안됨 - 전류 증가: %s A\r\n", (motor == L) ? "좌측" : "우측", Float2String(newCurrent, 1)); if (motor == L) Motor1.Cmd.Icmd.q = newCurrent; else Motor2.Cmd.Icmd.q = newCurrent; WP_ForcedCali.WasRotating[motor] = 0; // 회전 상태 리셋 } else { // 최대 전류(1.1A)에서도 회전 불가 - 케이블이 다 감긴 상태로 판단하여 완료 Debug_UART3_printf("%s 최대 전류에서 회전 불가 - 감기 완료로 판단\r\n", (motor == L) ? "좌측" : "우측"); if (motor == L) Motor1.Cmd.Icmd.q = 0.0f; else Motor2.Cmd.Icmd.q = 0.0f; Debug_UART3_printf("PositionCalibrate(%d) 호출 전\r\n", motor); PositionCalibrate(motor); // L(0) 또는 R(1) 직접 전달 Debug_UART3_printf("PositionCalibrate(%d) 호출 후\r\n", motor); WP_ForcedCali.Status[motor] = Done; } } WP_ForcedCali.StopTime[motor] = currentTime; // 리셋 } WP_ForcedCali.WasRotating[motor] = isRotating; } // 여기서 건드린 모터 제어 관련 변수는 모두 원복 해야 함 !!! // 성공 시 // if(debug_FrcCali) { // Drive_Status = RunGym; // Debug_DriveMotor = Drv_BothMotor; // cnt_FrcCali++; // } // 모터 제어 관련 변수 초기화할 것! } void CheckCoilTemp (void) { }