Bayley Wang
/
qonly_controller
derp
main.cpp@0:bac9c3a3a6ca, 2016-03-09 (annotated)
- Committer:
- bwang
- Date:
- Wed Mar 09 06:44:51 2016 +0000
- Revision:
- 0:bac9c3a3a6ca
- Child:
- 1:7b61790f6be9
open loop, working; motor draws ~400mA at 30V; pre-center aligned PWM
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:bac9c3a3a6ca | 1 | #include "mbed.h" |
bwang | 0:bac9c3a3a6ca | 2 | #include "math.h" |
bwang | 0:bac9c3a3a6ca | 3 | #include "PositionSensor.h" |
bwang | 0:bac9c3a3a6ca | 4 | #include "FastPWM.h" |
bwang | 0:bac9c3a3a6ca | 5 | #include "Transforms.h" |
bwang | 0:bac9c3a3a6ca | 6 | #include "config.h" |
bwang | 0:bac9c3a3a6ca | 7 | |
bwang | 0:bac9c3a3a6ca | 8 | FastPWM a(PWMA); |
bwang | 0:bac9c3a3a6ca | 9 | FastPWM b(PWMB); |
bwang | 0:bac9c3a3a6ca | 10 | FastPWM c(PWMC); |
bwang | 0:bac9c3a3a6ca | 11 | DigitalOut en(EN); |
bwang | 0:bac9c3a3a6ca | 12 | |
bwang | 0:bac9c3a3a6ca | 13 | AnalogOut test(TEST_DAC); |
bwang | 0:bac9c3a3a6ca | 14 | AnalogIn Ia(IA); |
bwang | 0:bac9c3a3a6ca | 15 | AnalogIn Ib(IB); |
bwang | 0:bac9c3a3a6ca | 16 | |
bwang | 0:bac9c3a3a6ca | 17 | PositionSensorEncoder pos(CPR, 0); |
bwang | 0:bac9c3a3a6ca | 18 | |
bwang | 0:bac9c3a3a6ca | 19 | Serial pc(USBTX, USBRX); |
bwang | 0:bac9c3a3a6ca | 20 | |
bwang | 0:bac9c3a3a6ca | 21 | using namespace Transforms; |
bwang | 0:bac9c3a3a6ca | 22 | |
bwang | 0:bac9c3a3a6ca | 23 | void config_globals() { |
bwang | 0:bac9c3a3a6ca | 24 | pc.baud(115200); |
bwang | 0:bac9c3a3a6ca | 25 | |
bwang | 0:bac9c3a3a6ca | 26 | a.period_us(200); |
bwang | 0:bac9c3a3a6ca | 27 | b.period_us(200); |
bwang | 0:bac9c3a3a6ca | 28 | c.period_us(200); |
bwang | 0:bac9c3a3a6ca | 29 | a = 1.0f; |
bwang | 0:bac9c3a3a6ca | 30 | b = 1.0f; |
bwang | 0:bac9c3a3a6ca | 31 | c = 1.0f; |
bwang | 0:bac9c3a3a6ca | 32 | |
bwang | 0:bac9c3a3a6ca | 33 | en = 1; |
bwang | 0:bac9c3a3a6ca | 34 | } |
bwang | 0:bac9c3a3a6ca | 35 | |
bwang | 0:bac9c3a3a6ca | 36 | void startup_msg() { |
bwang | 0:bac9c3a3a6ca | 37 | pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A."); |
bwang | 0:bac9c3a3a6ca | 38 | pc.printf("%s\n\r", "====Config Data===="); |
bwang | 0:bac9c3a3a6ca | 39 | pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET); |
bwang | 0:bac9c3a3a6ca | 40 | pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE); |
bwang | 0:bac9c3a3a6ca | 41 | pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE); |
bwang | 0:bac9c3a3a6ca | 42 | pc.printf("Loop KP: %f\n\r", KP); |
bwang | 0:bac9c3a3a6ca | 43 | pc.printf("Loop KI: %f\n\r", KI); |
bwang | 0:bac9c3a3a6ca | 44 | pc.printf("\n\r"); |
bwang | 0:bac9c3a3a6ca | 45 | } |
bwang | 0:bac9c3a3a6ca | 46 | |
bwang | 0:bac9c3a3a6ca | 47 | void main_loop() { |
bwang | 0:bac9c3a3a6ca | 48 | float p = pos.GetElecPosition() - POS_OFFSET + PI / 2; |
bwang | 0:bac9c3a3a6ca | 49 | if (p < 0) p += 2 * PI; |
bwang | 0:bac9c3a3a6ca | 50 | |
bwang | 0:bac9c3a3a6ca | 51 | float pos_dac = 0.85f * p / (2 * PI) + 0.05f; |
bwang | 0:bac9c3a3a6ca | 52 | |
bwang | 0:bac9c3a3a6ca | 53 | float v_ia = Ia.read() * AVDD; |
bwang | 0:bac9c3a3a6ca | 54 | float v_ib = Ib.read() * AVDD; |
bwang | 0:bac9c3a3a6ca | 55 | |
bwang | 0:bac9c3a3a6ca | 56 | float ia = (v_ia - I_OFFSET) / I_SCALE; |
bwang | 0:bac9c3a3a6ca | 57 | float ib = (v_ia - I_OFFSET) / I_SCALE; |
bwang | 0:bac9c3a3a6ca | 58 | |
bwang | 0:bac9c3a3a6ca | 59 | test = pos_dac; |
bwang | 0:bac9c3a3a6ca | 60 | |
bwang | 0:bac9c3a3a6ca | 61 | set_dtc(a, 0.5f + 0.5f * cosf(p)); |
bwang | 0:bac9c3a3a6ca | 62 | set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3)); |
bwang | 0:bac9c3a3a6ca | 63 | set_dtc(c, 0.5f + 0.5f * cosf(p - 2 * PI / 3)); |
bwang | 0:bac9c3a3a6ca | 64 | } |
bwang | 0:bac9c3a3a6ca | 65 | |
bwang | 0:bac9c3a3a6ca | 66 | int main() { |
bwang | 0:bac9c3a3a6ca | 67 | config_globals(); |
bwang | 0:bac9c3a3a6ca | 68 | startup_msg(); |
bwang | 0:bac9c3a3a6ca | 69 | |
bwang | 0:bac9c3a3a6ca | 70 | Ticker loop; |
bwang | 0:bac9c3a3a6ca | 71 | loop.attach_us(main_loop, 200); |
bwang | 0:bac9c3a3a6ca | 72 | |
bwang | 0:bac9c3a3a6ca | 73 | for (;;) { |
bwang | 0:bac9c3a3a6ca | 74 | } |
bwang | 0:bac9c3a3a6ca | 75 | } |