File content as of revision 3:a08e9ebf1958:

#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"
Ticker tickObject;

#include <iostream>

using namespace std;
const int n = 1000;
int k = 0;
int z = 0;
int prbstest[n];
int status;
int statusnew;
const int p = 2*n;
int positievec[p];
int q = 0;

/** Main function
 * Bootstraps the system
 */
typedef enum z_state{/*Z_ZERO,Z_B,Z_K,Z_OFF*/Z_I,Z_NUL}z_states;

void SetImpedance(float i, float b, float k, float pos)
{
            ZControl_I = i;
            ZControl_B = b;
            ZControl_K = k;
            ZControl_RefPos = pos;   
}

void blink(void)
{
    static z_states localstate=Z_I;
    switch(localstate)
    {
        /*case Z_ZERO:
        {
            localstate = Z_B; //mass
            SetImpedance(0,0.03,0,position);
            break;
        }
        case Z_B:
        {
            localstate = Z_I; //fluid
            SetImpedance(0.0009,0.01,0.001,position);
            break;
        }*/
        case Z_I:
        {
            localstate = Z_NUL;//spring
            SetImpedance(0.0001,0.05,0.8,0);
            break;
        }
        case Z_NUL:
        {
            localstate = Z_I;//spring
            SetImpedance(0.0001,0.05,0.8,500);
            break;
        }
        /*case Z_K:
        {
            localstate = Z_OFF;
            SetImpedance(0,0,0,position);
            driver_enable_a = 0;
            driver_enable_b = 0;
            break;
        }
        case Z_OFF:
        {
            localstate = Z_I;
            SetImpedance(0,0.00,0,position);
            driver_enable_a = 1;
            driver_enable_b = 1;
            break;
        }*/
        default:
        {
            localstate = Z_I;
            ZControl_I = 0;
            ZControl_B = 0;
            ZControl_K = 0;
            ZControl_RefPos = position;
        }
    }
    info_led_3 != info_led_3;
    wait_ms(300); //debounce
}
    
void printer(){
    int positie = GET_POSITION();
    positievec[q] = positie;
    cout << positievec[q] << ",";
    q = q+1;
    wait_ms(300);
    }
     
int main()
{
    // Initialize system
    //initialiseer_prbs();
    initialize_io();
    calibrate_current_sensor();
    calibrate_position();

    for (int i = 0; i <= n; i++) {
        prbstest[i] = rand() % 2;
        cout << prbstest[i] << ",";//"\r\n";
        }
              torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
        //tickObject.attach_us(&printer, 100000);  
    /*while(z<=n){   
        //torque_control();
        printer();
        //cout << z << ";\r\n";
        //z = z+1;
        wait_us(TORQUE_CONTROLLER_INTERVAL_US);
    }*/

    //send_report.length = 16;
    //recv_report.length = 16;

        cout << "\r\n";
        /*
        for(int k=0; k <= n; k++) {
         status = prbstest[k];
         int kplus = k+1;
         statusnew = prbstest[kplus];
         if (status == statusnew){
            //cout << "equal:\r\n";
            //cout << status << " " << k << "\r\n";
            //cout << statusnew << " " << kplus << "\r\n";
            }
         else if (status != statusnew){
            //cout << "unequal:\r\n";
            //cout << status << " " << k << "\r\n";
            //cout << statusnew << " " << kplus << "\r\n";
            blink();
            }
        }*/
    cout << positievec;
    
    while(1) {
        int32_t abspos = ABSPOS();
        printer();
        if(!user_btn)
            blink();
        //tickObject.attach(&blink, 5);*/
        
        //blink();
        //wait(1);  
        
        //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;
}

/*//create a prbs signal
void initialiseer_prbs() {
    int prbstest[n] = {};

    for (int i = 0; i <= n; i++) {
        prbstest[i] = rand() % 2;
        cout << prbstest[i];
    }
}*/

/** 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);
    //user_btn.rise(blink);
    pc.baud(9600);
    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;
        //cout << torque << ",";//"\r\n";

    // 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;
}