Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

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?

UserRevisionLine numberNew 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 }