This is a good example code for 2.74 lab 3 impedance control
Dependencies: ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 17:98e298577f09
- Parent:
- 16:bbe4ac38c535
- Child:
- 18:54195aa5e534
diff -r bbe4ac38c535 -r 98e298577f09 main.cpp --- a/main.cpp Sat Sep 12 16:10:06 2020 +0000 +++ b/main.cpp Fri Sep 25 01:07:22 2020 +0000 @@ -6,13 +6,24 @@ #include "MotorShield.h" #include "HardwareSetup.h" -#define NUM_INPUTS 2 -#define NUM_OUTPUTS 4 - +#define NUM_INPUTS 8 +#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; 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; 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) @@ -21,6 +32,36 @@ MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ +void current_control() +{ + 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 @@ -33,12 +74,28 @@ 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 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]; + + ControlLoop.attach(¤t_control,0.0001); //start current loop + // Setup experiment t.reset(); t.start(); + float th= 0; + float error = 0; encoderA.reset(); encoderB.reset(); encoderC.reset(); @@ -50,27 +107,29 @@ //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards. // Run experiment - while( t.read() < 2 ) { + while( t.read() < 5 ) { // Perform control loop logic - if (t.read() < 1) - motorShield.motorAWrite(d1, 0); //run motor A at "v1" duty cycle and in the forward direction - else - motorShield.motorAWrite(d2, 0); //run motor A at "v2" duty cycle and in the forward direction + 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; - // Form output to send to MATLAB float output_data[NUM_OUTPUTS]; - output_data[0] = t.read(); - output_data[1] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1 - output_data[2] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1 - output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15; + 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); - + 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