This is a good example code for 2.74 lab 3 impedance control
Dependencies: ExperimentServer QEI_pmw MotorShield
main.cpp
- Committer:
- sangbae
- Date:
- 2020-09-26
- Revision:
- 20:4c172a0737c6
- Parent:
- 19:d58bf34141e9
File content as of revision 20:4c172a0737c6:
#include "mbed.h" #include "rtos.h" #include "EthernetInterface.h" #include "ExperimentServer.h" #include "QEI.h" #include "MotorShield.h" #include "HardwareSetup.h" #define NUM_INPUTS 9 #define NUM_OUTPUTS 6 float velocity = 0; float current = 0; float current_d = 0; float volt = 0; float duty = 0; float cum_error = 0; float kp = 0; float ki = 0; float R =0; float emf =0; float th_d= 0; float period=0; Serial pc(USBTX, USBRX); // USB Serial Terminal ExperimentServer server; // Object that lets us communicate with MATLAB Timer t; // Timer to measure elapsed time of experiment Ticker ControlLoop; //Ticker object created QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding) QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding) QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding) QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding) MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ void current_control() //Current control function { float error = 0; velocity = encoderA.getVelocity()*(6.2831/1200.0); current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.15); //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab. error = current_d - current; cum_error = cum_error + error; if (cum_error < -3){ cum_error = -3; } else if (cum_error > 3){ cum_error = 3; } volt = R*current_d + emf*velocity + kp * error + ki * (cum_error); duty = volt/12.0; if (duty > 1){ duty = 1; } if (duty <-1){ duty = -1; } if (duty >= 0){ motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction } else if (duty < 0){ motorShield.motorAWrite(abs(duty), 1); } } int main (void) { // Link the terminal with our server and start it up server.attachTerminal(pc); server.init(); // Continually get input from MATLAB and run experiments float input_params[NUM_INPUTS]; pc.printf("%f",input_params[0]); while(1) { if (server.getParams(input_params,NUM_INPUTS)) { //float d1 = input_params[0]; // Duty cycle for first second //float d2 = input_params[1]; // Duty cycle for second second //float current_d = input_params[0]; //float volt_in = input_params[0]; current_d = 0; cum_error = 0; th_d =input_params[0]; R = input_params[1]; emf = input_params[2]; kp = input_params[3]; ki = input_params[4]; float k = input_params[5]; float b = input_params[6]; float D = input_params[7]; period = input_params[8]; ControlLoop.attach(¤t_control,period); //start current loop using Ticker function // Setup experiment t.reset(); t.start(); float th= 0; float error = 0; encoderA.reset(); encoderB.reset(); encoderC.reset(); encoderD.reset(); motorShield.motorAWrite(0, 0); //turn motor A off //use the motor shield as follows: //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards. // Run experiment while( t.read() < 5 ) { // Perform control loop logic th = encoderA.getPulses()*(6.2831/1200.0); //velocity = encoderA.getVelocity()*(6.2831/1200.0); //current = motorShield.readCurrentA()*(30.0/65536.0)-15.1; //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab. current_d = k*(th_d-th) - D*velocity + b*velocity; float output_data[NUM_OUTPUTS]; output_data[0] = t.read(); output_data[1] = th; output_data[2] = velocity; output_data[3] = current; output_data[4] = current_d; output_data[5] = volt; // Send data to MATLAB server.sendData(output_data,NUM_OUTPUTS); wait(.001); //run control loop at 1kHz } // Cleanup after experiment ControlLoop.detach(); server.setExperimentComplete(); motorShield.motorAWrite(0, 0); //turn motor A off } // end if } // end while } // end main