util.cpp

00001 #include "constants.h"
00002 #include "shared.h"
00003 #include "isr.h"
00004 #include <stdlib.h>
00005 
00006 void initTimers() {
00007 #ifdef __USE_THROTTLE
00008     dtc_upd_ticker.attach_us(&dtc_update, 200);
00009     throttle_upd_ticker.attach_us(&throttle_update, 100000);
00010 #endif
00011 
00012 #ifndef __NATIVE
00013     pha.period_us(100);
00014     phb.period_us(100);
00015     phc.period_us(100);
00016     pha = 0;
00017     phb = 1.0f;
00018     phc = 0;
00019 #endif
00020 
00021     TIM2->CR1 &= ~(TIM_CR1_CEN);
00022     TIM2->CR1 |= TIM_CR1_CMS;
00023     
00024 #ifdef __NATIVE
00025     TIM2->CCMR1 |= TIM_CCMR1_OC1M_2;
00026     TIM2->CCMR1 |= TIM_CCMR1_OC1M_1;
00027     TIM2->CCMR1 |= TIM_CCMR1_OC2M_2;
00028     TIM2->CCMR1 |= TIM_CCMR1_OC2M_1;
00029     TIM2->CCMR2 |= TIM_CCMR2_OC3M_2;
00030     TIM2->CCMR2 |= TIM_CCMR2_OC3M_1;
00031     
00032     TIM2->CCER &= ~TIM_CCER_CC1P;
00033     TIM2->CCER &= ~TIM_CCER_CC2P;
00034     TIM2->CCER &= ~TIM_CCER_CC3P;
00035     
00036     TIM2->CCER |= TIM_CCER_CC1E;
00037     TIM2->CCER |= TIM_CCER_CC2E;
00038     TIM2->CCER |= TIM_CCER_CC3E;
00039     
00040     TIM2->PSC = 0;
00041     
00042     TIM2->ARR = 10000;
00043     TIM2->CCR1 = 0;
00044     TIM2->CCR2 = 0;
00045     TIM2->CCR3 = 0;
00046 #endif
00047 
00048     TIM2->CR1 |= TIM_CR1_CEN;
00049 }
00050 
00051 void initPins() {        
00052     en = 0;
00053 }
00054 
00055 void initData() {
00056     motor = (Motor*) malloc(sizeof(Motor));
00057     motor->sensor_phase = SENSOR_PHASE;
00058     motor->halt = 0;
00059     motor->angle = 0;
00060     motor->debug_stop = 0;
00061 
00062 #ifdef __USE_THROTTLE
00063     float throttle_raw;
00064     do {
00065         throttle_raw = throttle;
00066         throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
00067         if (throttle_raw < 0.0f) throttle_raw = 0.0f;
00068     } while (throttle_raw > 0.05f);
00069     
00070     motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
00071     if (motor->throttle < 0.05f) {
00072         motor->halt = 1;
00073     } else {
00074         motor->halt = 0;
00075     }
00076 #else
00077     motor->throttle = 1.0f;
00078 #endif
00079 }
00080 
00081 void setDtcA(float f) {
00082 #ifdef __NATIVE
00083     TIM2->CCR3 = (uint16_t) (f * 10000.0f);
00084 #else
00085     pha = f;
00086 #endif
00087 }
00088 
00089 void setDtcB(float f) {
00090 #ifdef __NATIVE
00091     TIM2->CCR1 = (uint16_t) (f * 10000.0f);
00092 #else
00093     phb = f;
00094 #endif
00095 }
00096 
00097 void setDtcC(float f) {
00098 #ifdef __NATIVE
00099     TIM2->CCR2 = (uint16_t) (f * 10000.0f);
00100 #else
00101     phc = f;
00102 #endif
00103 }