File content as of revision 0:f3cf9865b7be:

#include "mbed.h"
#include "arm_math.h"
#include "USBHID.h"
#include <math.h>
#include <string>
#include <stdlib.h>
#include "MODSERIAL.h"
#include "speedestimator.h"
#include "position_sensor_error.h"
#include "cogging_compensation.h"
#include "main.h"

/** Main function
 * Bootstraps the system
 */
int main()
{
    // Initialize system
    initialize_io();
    calibrate_current_sensor();
    calibrate_position();

    torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
    
    send_report.length = 16;
    recv_report.length = 16;
    
    while(1){
                
        int32_t abspos = ABSPOS();
        
        send_report.data[3] = abspos & 0x000000ff;
        send_report.data[2] = (abspos & 0x0000ff00) >> 8;
        send_report.data[1] = (abspos & 0x00ff0000) >> 16;
        send_report.data[0] = (abspos & 0xff000000) >> 24;
        
        for(int i = 4; i < 16; i++){
            send_report.data[i] = 0x0;
        }
        
        //Send the report
        hid.send(&send_report);    
        
        // Try to read
        if(hid.readNB(&recv_report)) {
                
            ZControl_I          = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
            ZControl_B          = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
            ZControl_K          = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
            ZControl_RefPos     = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
        }
        
        info_led_3 = !info_led_3;
        wait(0.01);
    }

    return 0;
}

/** Sample the current sensor to determine the offset
 */
void calibrate_current_sensor()
{
    driver_enable_a = 0;
    driver_enable_b = 0;
    for(int i=0; i<100; i++) {
        current_sensor_a_offset+= 0.01f*current_sensor_a.read();
        current_sensor_b_offset+= 0.01f*current_sensor_b.read();
        wait_us(2000);
    }
    driver_enable_a = 1;
    driver_enable_b = 1;
}

/** Calibrates to find the reference position
 */
void calibrate_position()
{
    position_ref = 0;
    driver_vref_ab = 0.5;
    for(int i = 0; i < 10; i++) {
        driver_1a = 0.7;
        driver_2a = 0;
        driver_1b = 0;
        driver_2b = 0;
        wait(0.2);
        position_ref+= GET_POSITION();
        driver_1a = 0;
        driver_2a = 0;
        driver_1b = 0.7;
        driver_2b = 0;
        wait(0.01);
    }
    driver_vref_ab = 1;
    position_ref = position_ref/10;
    driver_1b = 0;
}

/** Initialize I/O (PWM, DigitalIn/Out, AnalogIn)
 */
void initialize_io()
{
    user_btn.mode(PullUp);
    pc.baud(115200);
    spi.format(14,3);
    driver_1a.period_us(33);
    driver_2a.period_us(33);
    driver_1b.period_us(33);
    driver_2b.period_us(33);
    driver_enable_a = 1;
    driver_enable_b = 1;
    driver_vref_ab  = 1;
}

/** Torque Controller function, controls the plant
 * This function is called on an interrupt basis by a Ticker object.
 * PI current controller and a Park transform for FOC
 */
void torque_control()
{

    // Get position
    static int   last_position = 0;
    static float last_speed = 0;
    static float position_sin;
    static float position_cos;
    static float position_theta;
    static float torque_setpoint;
    static int position_int;
    position        = GET_POSITION();
#if ENABLE_POSITION_COMPENSATION == 1
    position       += position_sensor_error[position];
#endif
    // Antialias
    if(position - last_position > POSITION_ANTIALIAS) {
        position_offset_count--;
        last_position+=8192;
    }
    if(position - last_position < -POSITION_ANTIALIAS) {
        position_offset_count++;
        last_position-=8192;
    }

    // Speed and position processing
    static speedEstimator speed_estimator(position);
    speed           = 0.00076699f*speed_estimator.get(position+POSITION_RESOLUTION*position_offset_count);  // rad/s
    LOWPASSIIR(acceleration, TORQUE_CONTROLLER_INTERVAL_INV*(speed - last_speed), 0.005f);
    last_position   = position;
    last_speed      = speed;
    position_theta  = fmod(1.0f*(position-position_ref+8192),163.84f);
    position_int    = floor(position_theta);
    position_theta *= ELECTRICAL_POSITION_TO_RAD;
    position_sin    = arm_sin_f32(position_theta);
    position_cos    = arm_cos_f32(position_theta);

    // Impedance controller...
    torque = -ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;

    // Preprocess torque command
    torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);
#if ENABLE_COGGING_COMPENSATION == 1
    torque_setpoint+= CC_GAIN*(cogging_compensation[position_int]);
#endif

    /**
     *F|   /    Stribeck + Coulomb + Viscous
     * |\_/
     * |____v_ */

#if ENABLE_FRICTION_COMPENSATION == 1
    torque_setpoint+= tanh(COULOMB_VELOCITY_CONST*speed) * (COULOMB_FRICTION + (STRIBECK_FRICTION-COULOMB_FRICTION)*exp(-abs(speed/STRIBECK_VELOCITY_CONST))) + (speed > 0 ? VISCOUS_FRICTION_COEF_FWD : VISCOUS_FRICTION_COEF_REV)*speed;
#endif
#if ENABLE_DITHER == 1
    dither_tick++;
    if(dither_tick > DITHER_TICKS) {
        dither_tick = 0;
    } else {
        torque_setpoint+=DITHER_FORCE*sin(2*PI/DITHER_TICKS*dither_tick);
    }
#endif

    // Transform torque command
    static float current_a_setpoint;
    static float current_b_setpoint;
    arm_inv_park_f32(0, torque_setpoint, &current_a_setpoint, &current_b_setpoint, position_sin, position_cos);

    // PI Controller
    static float current_a_error;
    static float current_b_error;
    static float current_a_int_error = 0;
    static float current_b_int_error = 0;
    current_a_error = current_a_setpoint - GET_CURRENT_A();
    current_b_error = current_b_setpoint - GET_CURRENT_B();

    if(!(current_a_int_error > CURRENT_CONTROLLER_I_LIMIT && current_a_error > 0) && !(current_a_int_error < -CURRENT_CONTROLLER_I_LIMIT && current_a_error < 0))
        current_a_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_a_error;
    if(!(current_b_int_error > CURRENT_CONTROLLER_I_LIMIT && current_b_error > 0) && !(current_b_int_error < -CURRENT_CONTROLLER_I_LIMIT && current_b_error < 0))
        current_b_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_b_error;

    current_a_error *= CURRENT_CONTROLLER_KP;
    current_b_error *= CURRENT_CONTROLLER_KP;
    current_a_error += CURRENT_CONTROLLER_KI*current_a_int_error;
    current_b_error += CURRENT_CONTROLLER_KI*current_b_int_error;

    // Apply voltages
    driver_1a   = current_a_error > 0   ? current_a_error   : 0;
    driver_2a   = current_a_error < 0   ? -current_a_error  : 0;
    driver_1b   = current_b_error > 0   ? current_b_error   : 0;
    driver_2b   = current_b_error < 0   ? -current_b_error  : 0;
}