N K / Mbed 2 deprecated analoghalls_part_4

yup

Dependencies:   mbed

Fork of analoghalls by Bayley Wang

util.cpp@6:4960629abb90, 2015-02-26 (annotated)

Committer:
nki
Date:
Thu Feb 26 14:09:19 2015 +0000
Revision:
6:4960629abb90
Parent:
5:eeb8af99cb6c 
LOLOL PROPROTIONAL CURENT CONTROL;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nki 0:9753f3c2e5ca 1 #include "constants.h"
nki 0:9753f3c2e5ca 2 #include "shared.h"
nki 0:9753f3c2e5ca 3 #include "isr.h"
nki 0:9753f3c2e5ca 4 #include <stdlib.h>
nki 0:9753f3c2e5ca 5
nki 0:9753f3c2e5ca 6 void initTimers() {
bwang 2:b5c19d4eddcc 7 #ifdef __USE_THROTTLE
bwang 1:70eed554399b 8 dtc_upd_ticker.attach_us(&dtc_update, 200);
bwang 1:70eed554399b 9 throttle_upd_ticker.attach_us(&throttle_update, 100000);
nki 5:eeb8af99cb6c 10 isense_upd_ticker.attach_us(&isense_update,1000);
bwang 2:b5c19d4eddcc 11 #endif
bwang 2:b5c19d4eddcc 12
bwang 2:b5c19d4eddcc 13 #ifndef __NATIVE
bwang 2:b5c19d4eddcc 14 pha.period_us(100);
bwang 2:b5c19d4eddcc 15 phb.period_us(100);
bwang 2:b5c19d4eddcc 16 phc.period_us(100);
nki 0:9753f3c2e5ca 17 pha = 0;
nki 0:9753f3c2e5ca 18 phb = 1.0f;
nki 0:9753f3c2e5ca 19 phc = 0;
bwang 2:b5c19d4eddcc 20 #endif
nki 0:9753f3c2e5ca 21
bwang 2:b5c19d4eddcc 22 TIM2->CR1 &= ~(TIM_CR1_CEN);
bwang 2:b5c19d4eddcc 23 TIM2->CR1 |= TIM_CR1_CMS;
bwang 2:b5c19d4eddcc 24
bwang 2:b5c19d4eddcc 25 #ifdef __NATIVE
bwang 2:b5c19d4eddcc 26 TIM2->CCMR1 |= TIM_CCMR1_OC1M_2;
bwang 2:b5c19d4eddcc 27 TIM2->CCMR1 |= TIM_CCMR1_OC1M_1;
bwang 2:b5c19d4eddcc 28 TIM2->CCMR1 |= TIM_CCMR1_OC2M_2;
bwang 2:b5c19d4eddcc 29 TIM2->CCMR1 |= TIM_CCMR1_OC2M_1;
bwang 2:b5c19d4eddcc 30 TIM2->CCMR2 |= TIM_CCMR2_OC3M_2;
bwang 2:b5c19d4eddcc 31 TIM2->CCMR2 |= TIM_CCMR2_OC3M_1;
bwang 2:b5c19d4eddcc 32
bwang 2:b5c19d4eddcc 33 TIM2->CCER &= ~TIM_CCER_CC1P;
bwang 2:b5c19d4eddcc 34 TIM2->CCER &= ~TIM_CCER_CC2P;
bwang 2:b5c19d4eddcc 35 TIM2->CCER &= ~TIM_CCER_CC3P;
bwang 2:b5c19d4eddcc 36
bwang 2:b5c19d4eddcc 37 TIM2->CCER |= TIM_CCER_CC1E;
bwang 2:b5c19d4eddcc 38 TIM2->CCER |= TIM_CCER_CC2E;
bwang 2:b5c19d4eddcc 39 TIM2->CCER |= TIM_CCER_CC3E;
bwang 2:b5c19d4eddcc 40
bwang 2:b5c19d4eddcc 41 TIM2->PSC = 0;
bwang 2:b5c19d4eddcc 42
bwang 2:b5c19d4eddcc 43 TIM2->ARR = 10000;
bwang 2:b5c19d4eddcc 44 TIM2->CCR1 = 0;
bwang 2:b5c19d4eddcc 45 TIM2->CCR2 = 0;
bwang 2:b5c19d4eddcc 46 TIM2->CCR3 = 0;
bwang 2:b5c19d4eddcc 47 #endif
bwang 2:b5c19d4eddcc 48
bwang 2:b5c19d4eddcc 49 TIM2->CR1 |= TIM_CR1_CEN;
nki 0:9753f3c2e5ca 50 }
nki 0:9753f3c2e5ca 51
bwang 1:70eed554399b 52 void initPins() {
nki 0:9753f3c2e5ca 53 en = 0;
nki 0:9753f3c2e5ca 54 }
nki 0:9753f3c2e5ca 55
nki 0:9753f3c2e5ca 56 void initData() {
nki 0:9753f3c2e5ca 57 motor = (Motor*) malloc(sizeof(Motor));
nki 0:9753f3c2e5ca 58 motor->sensor_phase = SENSOR_PHASE;
bwang 2:b5c19d4eddcc 59 motor->halt = 0;
bwang 2:b5c19d4eddcc 60 motor->angle = 0;
bwang 3:86ccde39f61b 61 motor->debug_stop = 0;
bwang 1:70eed554399b 62
bwang 2:b5c19d4eddcc 63 #ifdef __USE_THROTTLE
nki 0:9753f3c2e5ca 64 float throttle_raw;
nki 0:9753f3c2e5ca 65 do {
nki 0:9753f3c2e5ca 66 throttle_raw = throttle;
nki 0:9753f3c2e5ca 67 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
nki 0:9753f3c2e5ca 68 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
nki 0:9753f3c2e5ca 69 } while (throttle_raw > 0.05f);
nki 0:9753f3c2e5ca 70
nki 0:9753f3c2e5ca 71 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
nki 5:eeb8af99cb6c 72 if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) {
nki 0:9753f3c2e5ca 73 motor->halt = 1;
nki 0:9753f3c2e5ca 74 } else {
nki 0:9753f3c2e5ca 75 motor->halt = 0;
nki 0:9753f3c2e5ca 76 }
bwang 2:b5c19d4eddcc 77 #else
bwang 2:b5c19d4eddcc 78 motor->throttle = 1.0f;
bwang 2:b5c19d4eddcc 79 #endif
bwang 2:b5c19d4eddcc 80 }
bwang 2:b5c19d4eddcc 81
bwang 2:b5c19d4eddcc 82 void setDtcA(float f) {
bwang 2:b5c19d4eddcc 83 #ifdef __NATIVE
bwang 2:b5c19d4eddcc 84 TIM2->CCR3 = (uint16_t) (f * 10000.0f);
bwang 2:b5c19d4eddcc 85 #else
bwang 2:b5c19d4eddcc 86 pha = f;
bwang 2:b5c19d4eddcc 87 #endif
bwang 2:b5c19d4eddcc 88 }
bwang 2:b5c19d4eddcc 89
bwang 2:b5c19d4eddcc 90 void setDtcB(float f) {
bwang 2:b5c19d4eddcc 91 #ifdef __NATIVE
bwang 2:b5c19d4eddcc 92 TIM2->CCR1 = (uint16_t) (f * 10000.0f);
bwang 2:b5c19d4eddcc 93 #else
bwang 2:b5c19d4eddcc 94 phb = f;
bwang 2:b5c19d4eddcc 95 #endif
bwang 2:b5c19d4eddcc 96 }
bwang 2:b5c19d4eddcc 97
bwang 2:b5c19d4eddcc 98 void setDtcC(float f) {
bwang 2:b5c19d4eddcc 99 #ifdef __NATIVE
bwang 2:b5c19d4eddcc 100 TIM2->CCR2 = (uint16_t) (f * 10000.0f);
bwang 2:b5c19d4eddcc 101 #else
bwang 2:b5c19d4eddcc 102 phc = f;
bwang 2:b5c19d4eddcc 103 #endif
nki 0:9753f3c2e5ca 104 }