Revision 0:bac9c3a3a6ca, committed 2016-03-09

Comitter:

bwang

Date:

Wed Mar 09 06:44:51 2016 +0000

Child:

1:7b61790f6be9

Commit message:

open loop, working; motor draws ~400mA at 30V; pre-center aligned PWM

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/benkatz/code/FastPWM3/#51c979bca21e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,92 @@
+
+#include "mbed.h"
+#include "PositionSensor.h"
+#include <math.h>
+
+/*
+ * CPR: counts per revolution (4x lines per revolution)
+ * offset: mechanical position offset in radians
+ */
+    
+PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
+    _CPR = CPR;
+    _offset = offset;
+    
+    __GPIOA_CLK_ENABLE();
+    __GPIOB_CLK_ENABLE();
+ 
+    GPIOB->MODER   |= GPIO_MODER_MODER3_1;           
+    GPIOB->OTYPER  |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;                 
+    GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;     
+    GPIOB->AFR[0]  |= 0x00001000 ;                                          
+    
+    GPIOA->MODER   |= GPIO_MODER_MODER15_1;           
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_15;                 
+    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;     
+    GPIOA->AFR[1]  |= 0x10000000 ;
+   
+    __TIM2_CLK_ENABLE();
+ 
+    TIM2->CR1   = 0x0001;
+    TIM2->SMCR  = TIM_ENCODERMODE_TI12;
+    TIM2->CCMR1 = 0xf1f1;
+    TIM2->CCMR2 = 0x0000;
+    TIM2->CCER  = 0x0011;
+    TIM2->PSC   = 0x0000;
+    TIM2->ARR   = 0xffffffff;
+  
+    TIM2->CNT = 0;
+    
+    ZPulse = new InterruptIn(PB_12);
+    ZSense = new DigitalIn(PB_12);
+    ZPulse->enable_irq();
+    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
+    ZPulse->mode(PullDown);
+    
+    ZTest = new DigitalOut(PC_10);
+    ZTest->write(1);
+    state = 0;
+}
+
+/*
+ * Returns mechanical position in radians
+ */
+ 
+float PositionSensorEncoder::GetMechPosition() {
+    int raw = TIM2->CNT;
+    if (raw < 0) raw += _CPR;
+    if (raw >= _CPR) raw -= _CPR;
+    float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
+    if (signed_elec < 0){
+        return signed_elec + 6.28318530718f;
+    }
+    else{
+        return signed_elec;
+    }  
+}
+
+/*
+ * Returns electrical position in radians
+ */
+ 
+float PositionSensorEncoder::GetElecPosition() {
+    int raw = TIM2->CNT;
+    if (raw < 0) raw += _CPR;
+    if (raw >= _CPR) raw -= _CPR;
+    float signed_elec = fmod((2.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
+    if (signed_elec < 0) {
+        return signed_elec + 6.28318530718f;
+    } else {
+        return signed_elec;
+    }
+}
+
+void PositionSensorEncoder::ZeroEncoderCount(void){
+    if (ZSense->read() == 1){
+        if (ZSense->read() == 1){
+            TIM2->CNT=0;
+            state = !state;
+            ZTest->write(state);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,26 @@
+#ifndef POSITIONSENSOR_H
+#define POSITIONSENSOR_H
+
+class PositionSensor {
+public:
+    virtual float GetMechPosition() {return 0.0f;}
+    virtual float GetElecPosition() {return 0.0f;}
+};
+  
+  
+class PositionSensorEncoder: public PositionSensor {
+public:
+    PositionSensorEncoder(int CPR, float offset);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+private:
+    InterruptIn *ZPulse;
+    DigitalIn *ZSense;
+    DigitalOut *ZTest;
+    void ZeroEncoderCount(void);
+    int _CPR;
+    int state;
+    float _offset;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Transforms/Transforms.cpp	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,28 @@
+#include "mbed.h"
+#include "Transforms.h"
+
+void Transforms::Park(float alpha, float beta, float theta, float *d, float *q){
+    float cosine = cos(theta);
+    float sine = sin(theta);
+    *d = alpha * cosine - beta * sine;
+    *q = -beta * cosine - alpha * sine;
+}
+
+void Transforms::InvPark(float d, float q, float theta, float *alpha, float *beta){
+    float cosine = cos(theta);
+    float sine = sin(theta);
+    *alpha = d * cosine - q * sine;
+    *beta = q * cosine + d * sine;
+}
+
+void Transforms::Clarke(float a, float b, float *alpha, float *beta){
+    *alpha = a;
+    *beta = sqrtf(3.0f) / 3.0f * (a + 2.0f * b);
+}
+
+void Transforms::InvClarke(float alpha, float beta, float *a, float *b, float *c){
+    *a = alpha;
+    *b = 0.5f * (-alpha + sqrtf(3.0f) * beta);
+    *c = 0.5f * (-alpha - sqrtf(3.0f) * beta);
+} 
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Transforms/Transforms.h	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,8 @@
+namespace Transforms{
+    
+    void Park(float alpha, float beta, float theta, float *d, float *q);
+    void InvPark(float d, float q, float theta, float *alpha, float *beta);
+    void Clarke(float a, float b, float *alpha, float *beta);
+    void InvClarke(float alpha, float beta, float *a, float *b, float *c);
+};
+    

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,35 @@
+#ifndef __CONFIG_H
+#define __CONFIG_H
+
+#define set_dtc(phase, value) phase = 1.0f - (value)
+
+#define PWMA PA_8
+#define PWMB PA_9
+#define PWMC PA_10
+#define EN PB_15
+
+#define IA PA_4
+#define IB PB_0
+#define TEST_DAC PA_5
+
+#define PI 3.141593f
+#define CPR 4096
+#define POS_OFFSET 5.3f
+
+#define I_SCALE_RAW 25.0f //mv/A
+#define R_UP 12000.0f //ohms
+#define R_DOWN 3600.0f //ohms
+#define R_BIAS 3600.0f //ohms
+#define AVDD 3300.0f //mV
+
+#define I_OFFSET (AVDD * R_DOWN * R_UP / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
+#define I_SCALE (R_BIAS * R_DOWN * I_SCALE_RAW / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
+
+#define K_LOOP 5.0f
+#define KI_BASE 0.01f
+#define BUS_VOLTAGE 20.0f
+
+#define KP (K_LOOP / BUS_VOLTAGE)
+#define KI (KI_BASE * K_LOOP / BUS_VOLTAGE)
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "math.h"
+#include "PositionSensor.h"
+#include "FastPWM.h"
+#include "Transforms.h"
+#include "config.h"
+
+FastPWM a(PWMA);
+FastPWM b(PWMB);
+FastPWM c(PWMC);
+DigitalOut en(EN);
+
+AnalogOut test(TEST_DAC);
+AnalogIn Ia(IA);
+AnalogIn Ib(IB);
+
+PositionSensorEncoder pos(CPR, 0);
+
+Serial pc(USBTX, USBRX);
+
+using namespace Transforms;
+
+void config_globals() {
+    pc.baud(115200);
+    
+    a.period_us(200);
+    b.period_us(200);
+    c.period_us(200);
+    a = 1.0f;
+    b = 1.0f;
+    c = 1.0f;
+    
+    en = 1;
+}
+
+void startup_msg() {
+    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
+    pc.printf("%s\n\r", "====Config Data====");
+    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
+    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
+    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
+    pc.printf("Loop KP: %f\n\r", KP);
+    pc.printf("Loop KI: %f\n\r", KI);
+    pc.printf("\n\r");
+}    
+
+void main_loop() {
+    float p = pos.GetElecPosition() - POS_OFFSET + PI / 2;
+    if (p < 0) p += 2 * PI;
+    
+    float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
+    
+    float v_ia = Ia.read() * AVDD;
+    float v_ib = Ib.read() * AVDD;
+    
+    float ia = (v_ia - I_OFFSET) / I_SCALE;
+    float ib = (v_ia - I_OFFSET) / I_SCALE;
+    
+    test = pos_dac;
+    
+    set_dtc(a, 0.5f + 0.5f * cosf(p));
+    set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3));
+    set_dtc(c, 0.5f + 0.5f * cosf(p - 2 * PI / 3));
+}
+    
+int main() {
+    config_globals();
+    startup_msg();
+    
+    Ticker loop;
+    loop.attach_us(main_loop, 200);
+    
+    for (;;) {
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb
\ No newline at end of file