N K
/
analoghalls_part_4
yup
Fork of analoghalls by
util.cpp@0:9753f3c2e5ca, 2015-02-16 (annotated)
- Committer:
- nki
- Date:
- Mon Feb 16 20:16:01 2015 +0000
- Revision:
- 0:9753f3c2e5ca
- Child:
- 1:70eed554399b
hello bayley;
Who changed what in which revision?
User | Revision | Line number | New 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() { |
nki | 0:9753f3c2e5ca | 7 | dtc_upd_ticker.attach_us(&dtc_update, 20); |
nki | 0:9753f3c2e5ca | 8 | pha.period_us(200); |
nki | 0:9753f3c2e5ca | 9 | phb.period_us(200); |
nki | 0:9753f3c2e5ca | 10 | phc.period_us(200); |
nki | 0:9753f3c2e5ca | 11 | pha = 0; |
nki | 0:9753f3c2e5ca | 12 | phb = 1.0f; |
nki | 0:9753f3c2e5ca | 13 | phc = 0; |
nki | 0:9753f3c2e5ca | 14 | |
nki | 0:9753f3c2e5ca | 15 | } |
nki | 0:9753f3c2e5ca | 16 | |
nki | 0:9753f3c2e5ca | 17 | void initPins() { |
nki | 0:9753f3c2e5ca | 18 | halla.mode(PullUp); |
nki | 0:9753f3c2e5ca | 19 | hallb.mode(PullUp); |
nki | 0:9753f3c2e5ca | 20 | hallc.mode(PullUp); |
nki | 0:9753f3c2e5ca | 21 | |
nki | 0:9753f3c2e5ca | 22 | en = 0; |
nki | 0:9753f3c2e5ca | 23 | } |
nki | 0:9753f3c2e5ca | 24 | |
nki | 0:9753f3c2e5ca | 25 | void initData() { |
nki | 0:9753f3c2e5ca | 26 | motor = (Motor*) malloc(sizeof(Motor)); |
nki | 0:9753f3c2e5ca | 27 | motor->hall = (halla.read() << 2) + (hallb.read() << 1) + hallc.read(); |
nki | 0:9753f3c2e5ca | 28 | motor->last_hall = 0; |
nki | 0:9753f3c2e5ca | 29 | motor->reverses = 0; |
nki | 0:9753f3c2e5ca | 30 | motor->last_time = 3000.0f; |
nki | 0:9753f3c2e5ca | 31 | motor->major_pos = 60.0f * ATable[motor->hall]; |
nki | 0:9753f3c2e5ca | 32 | motor->ticks = 0.0f; |
nki | 0:9753f3c2e5ca | 33 | motor->sensor_phase = SENSOR_PHASE; |
nki | 0:9753f3c2e5ca | 34 | motor->halt = 1; |
nki | 0:9753f3c2e5ca | 35 | motor->whangle =0; |
nki | 0:9753f3c2e5ca | 36 | motor->analoga = analoga; |
nki | 0:9753f3c2e5ca | 37 | motor->analogb = analogb; |
nki | 0:9753f3c2e5ca | 38 | |
nki | 0:9753f3c2e5ca | 39 | float throttle_raw; |
nki | 0:9753f3c2e5ca | 40 | do { |
nki | 0:9753f3c2e5ca | 41 | throttle_raw = throttle; |
nki | 0:9753f3c2e5ca | 42 | throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB); |
nki | 0:9753f3c2e5ca | 43 | if (throttle_raw < 0.0f) throttle_raw = 0.0f; |
nki | 0:9753f3c2e5ca | 44 | } while (throttle_raw > 0.05f); |
nki | 0:9753f3c2e5ca | 45 | |
nki | 0:9753f3c2e5ca | 46 | motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle; |
nki | 0:9753f3c2e5ca | 47 | if (motor->throttle < 0.05f) { |
nki | 0:9753f3c2e5ca | 48 | motor->halt = 1; |
nki | 0:9753f3c2e5ca | 49 | } else { |
nki | 0:9753f3c2e5ca | 50 | motor->halt = 0; |
nki | 0:9753f3c2e5ca | 51 | } |
nki | 0:9753f3c2e5ca | 52 | } |