PRBS signal on Haptic_hid with output signal of position
Dependencies: MODSERIAL USBDevice compensation_tables mbed-dsp mbed
Fork of haptic_hid by
main.cpp
- Committer:
- vsluiter
- Date:
- 2017-06-19
- Revision:
- 4:9d37f163d64c
- Parent:
- 3:10863117020c
File content as of revision 4:9d37f163d64c:
#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> /* * Comment victor: this is defined in main.h * MODSERIAL pc(USBTX,USBRX); */ 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.00033,0.00033,0.0000034,0); break; } case Z_NUL: { localstate = Z_I;//spring SetImpedance(0.00033,0.00033,0.0000034,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] << ","; pc.printf("%d,", positievec[q]); q = q+1; if(q>=n) q=0; } int main() { // Initialize system //initialiseer_prbs(); pc.baud(115200); initialize_io(); calibrate_current_sensor(); calibrate_position(); for (int i = 0; i <= n; i++) { prbstest[i] = rand() % 2; pc.printf("%d,", prbstest[i]); } while(z<=n){ //torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US); //tickObject.attach_us(&printer, 100000); torque_control(); printer(); wait_ms(300); pc.printf("%d;\r\n",z); 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(); } }*/ for(int i=0 ; i<n ; i++) { pc.printf("%d ",positievec[i]); } while(1) { int32_t abspos = ABSPOS(); /*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... if(prbstest[z] == 1){ torque = -0.1;//-ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration; //cout << torque << ",";//"\r\n"; } else if(prbstest[z] == 0){ torque = 0.1; //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, ¤t_a_setpoint, ¤t_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; }