This is a good example code for 2.74 lab 3 impedance control
Dependencies: ExperimentServer QEI_pmw MotorShield
main.cpp@19:d58bf34141e9, 2020-09-26 (annotated)
- Committer:
- sangbae
- Date:
- Sat Sep 26 00:17:32 2020 +0000
- Revision:
- 19:d58bf34141e9
- Parent:
- 18:54195aa5e534
- Child:
- 20:4c172a0737c6
Minor update with comments
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pwensing | 0:43448bf056e8 | 1 | #include "mbed.h" |
pwensing | 0:43448bf056e8 | 2 | #include "rtos.h" |
pwensing | 0:43448bf056e8 | 3 | #include "EthernetInterface.h" |
pwensing | 0:43448bf056e8 | 4 | #include "ExperimentServer.h" |
pwensing | 0:43448bf056e8 | 5 | #include "QEI.h" |
elijahsj | 6:1faceb53dabe | 6 | #include "MotorShield.h" |
elijahsj | 13:3a1f4e09789b | 7 | #include "HardwareSetup.h" |
pwensing | 0:43448bf056e8 | 8 | |
sangbae | 18:54195aa5e534 | 9 | #define NUM_INPUTS 9 |
sangbae | 17:98e298577f09 | 10 | #define NUM_OUTPUTS 6 |
sangbae | 17:98e298577f09 | 11 | float velocity = 0; |
sangbae | 17:98e298577f09 | 12 | float current = 0; |
sangbae | 17:98e298577f09 | 13 | float current_d = 0; |
sangbae | 17:98e298577f09 | 14 | float volt = 0; |
sangbae | 17:98e298577f09 | 15 | float duty = 0; |
sangbae | 17:98e298577f09 | 16 | float cum_error = 0; |
sangbae | 17:98e298577f09 | 17 | float kp = 0; |
sangbae | 17:98e298577f09 | 18 | float ki = 0; |
sangbae | 17:98e298577f09 | 19 | float R =0; |
sangbae | 17:98e298577f09 | 20 | float emf =0; |
sangbae | 17:98e298577f09 | 21 | float th_d= 0; |
sangbae | 18:54195aa5e534 | 22 | float period=0; |
pwensing | 0:43448bf056e8 | 23 | |
pwensing | 0:43448bf056e8 | 24 | Serial pc(USBTX, USBRX); // USB Serial Terminal |
pwensing | 0:43448bf056e8 | 25 | ExperimentServer server; // Object that lets us communicate with MATLAB |
elijahsj | 5:1ab9b2527794 | 26 | Timer t; // Timer to measure elapsed time of experiment |
sangbae | 17:98e298577f09 | 27 | Ticker ControlLoop; |
elijahsj | 5:1ab9b2527794 | 28 | |
elijahsj | 5:1ab9b2527794 | 29 | QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding) |
elijahsj | 5:1ab9b2527794 | 30 | QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding) |
elijahsj | 5:1ab9b2527794 | 31 | QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding) |
elijahsj | 5:1ab9b2527794 | 32 | QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding) |
elijahsj | 5:1ab9b2527794 | 33 | |
elijahsj | 16:bbe4ac38c535 | 34 | MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ |
elijahsj | 6:1faceb53dabe | 35 | |
sangbae | 19:d58bf34141e9 | 36 | void current_control() //Current control function |
sangbae | 17:98e298577f09 | 37 | { |
sangbae | 17:98e298577f09 | 38 | float error = 0; |
sangbae | 17:98e298577f09 | 39 | velocity = encoderA.getVelocity()*(6.2831/1200.0); |
sangbae | 17:98e298577f09 | 40 | 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. |
sangbae | 17:98e298577f09 | 41 | error = current_d - current; |
sangbae | 17:98e298577f09 | 42 | cum_error = cum_error + error; |
sangbae | 17:98e298577f09 | 43 | if (cum_error < -3){ |
sangbae | 17:98e298577f09 | 44 | cum_error = -3; |
sangbae | 17:98e298577f09 | 45 | } |
sangbae | 17:98e298577f09 | 46 | else if (cum_error > 3){ |
sangbae | 17:98e298577f09 | 47 | cum_error = 3; |
sangbae | 17:98e298577f09 | 48 | } |
sangbae | 17:98e298577f09 | 49 | volt = R*current_d + emf*velocity + kp * error + ki * (cum_error); |
sangbae | 17:98e298577f09 | 50 | duty = volt/12.0; |
sangbae | 17:98e298577f09 | 51 | if (duty > 1){ |
sangbae | 17:98e298577f09 | 52 | duty = 1; |
sangbae | 17:98e298577f09 | 53 | } |
sangbae | 17:98e298577f09 | 54 | if (duty <-1){ |
sangbae | 17:98e298577f09 | 55 | duty = -1; |
sangbae | 17:98e298577f09 | 56 | } |
sangbae | 17:98e298577f09 | 57 | if (duty >= 0){ |
sangbae | 17:98e298577f09 | 58 | motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction |
sangbae | 17:98e298577f09 | 59 | } |
sangbae | 17:98e298577f09 | 60 | else if (duty < 0){ |
sangbae | 17:98e298577f09 | 61 | motorShield.motorAWrite(abs(duty), 1); |
sangbae | 17:98e298577f09 | 62 | } |
sangbae | 17:98e298577f09 | 63 | } |
sangbae | 17:98e298577f09 | 64 | |
sangbae | 17:98e298577f09 | 65 | |
elijahsj | 4:7a1b35f081bb | 66 | int main (void) |
elijahsj | 4:7a1b35f081bb | 67 | { |
pwensing | 0:43448bf056e8 | 68 | // Link the terminal with our server and start it up |
pwensing | 0:43448bf056e8 | 69 | server.attachTerminal(pc); |
pwensing | 0:43448bf056e8 | 70 | server.init(); |
elijahsj | 13:3a1f4e09789b | 71 | |
pwensing | 0:43448bf056e8 | 72 | // Continually get input from MATLAB and run experiments |
pwensing | 0:43448bf056e8 | 73 | float input_params[NUM_INPUTS]; |
elijahsj | 5:1ab9b2527794 | 74 | pc.printf("%f",input_params[0]); |
elijahsj | 5:1ab9b2527794 | 75 | |
pwensing | 0:43448bf056e8 | 76 | while(1) { |
pwensing | 0:43448bf056e8 | 77 | if (server.getParams(input_params,NUM_INPUTS)) { |
sangbae | 17:98e298577f09 | 78 | //float d1 = input_params[0]; // Duty cycle for first second |
sangbae | 17:98e298577f09 | 79 | //float d2 = input_params[1]; // Duty cycle for second second |
sangbae | 17:98e298577f09 | 80 | //float current_d = input_params[0]; |
sangbae | 17:98e298577f09 | 81 | //float volt_in = input_params[0]; |
sangbae | 17:98e298577f09 | 82 | current_d = 0; |
sangbae | 17:98e298577f09 | 83 | cum_error = 0; |
sangbae | 17:98e298577f09 | 84 | th_d =input_params[0]; |
sangbae | 17:98e298577f09 | 85 | R = input_params[1]; |
sangbae | 17:98e298577f09 | 86 | emf = input_params[2]; |
sangbae | 17:98e298577f09 | 87 | kp = input_params[3]; |
sangbae | 17:98e298577f09 | 88 | ki = input_params[4]; |
sangbae | 17:98e298577f09 | 89 | float k = input_params[5]; |
sangbae | 17:98e298577f09 | 90 | float b = input_params[6]; |
sangbae | 17:98e298577f09 | 91 | float D = input_params[7]; |
sangbae | 18:54195aa5e534 | 92 | period = input_params[8]; |
sangbae | 17:98e298577f09 | 93 | |
sangbae | 18:54195aa5e534 | 94 | ControlLoop.attach(¤t_control,period); //start current loop |
sangbae | 17:98e298577f09 | 95 | |
pwensing | 0:43448bf056e8 | 96 | // Setup experiment |
pwensing | 0:43448bf056e8 | 97 | t.reset(); |
pwensing | 0:43448bf056e8 | 98 | t.start(); |
sangbae | 17:98e298577f09 | 99 | float th= 0; |
sangbae | 17:98e298577f09 | 100 | float error = 0; |
elijahsj | 5:1ab9b2527794 | 101 | encoderA.reset(); |
elijahsj | 5:1ab9b2527794 | 102 | encoderB.reset(); |
elijahsj | 5:1ab9b2527794 | 103 | encoderC.reset(); |
elijahsj | 5:1ab9b2527794 | 104 | encoderD.reset(); |
elijahsj | 10:a40d180c305c | 105 | |
elijahsj | 15:495333b2ccf1 | 106 | motorShield.motorAWrite(0, 0); //turn motor A off |
elijahsj | 15:495333b2ccf1 | 107 | |
elijahsj | 15:495333b2ccf1 | 108 | //use the motor shield as follows: |
elijahsj | 15:495333b2ccf1 | 109 | //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards. |
elijahsj | 15:495333b2ccf1 | 110 | |
pwensing | 0:43448bf056e8 | 111 | // Run experiment |
sangbae | 17:98e298577f09 | 112 | while( t.read() < 5 ) { |
pwensing | 0:43448bf056e8 | 113 | // Perform control loop logic |
sangbae | 17:98e298577f09 | 114 | th = encoderA.getPulses()*(6.2831/1200.0); |
sangbae | 17:98e298577f09 | 115 | //velocity = encoderA.getVelocity()*(6.2831/1200.0); |
sangbae | 17:98e298577f09 | 116 | //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. |
sangbae | 17:98e298577f09 | 117 | |
sangbae | 17:98e298577f09 | 118 | current_d = k*(th_d-th) - D*velocity + b*velocity; |
elijahsj | 4:7a1b35f081bb | 119 | |
pwensing | 0:43448bf056e8 | 120 | float output_data[NUM_OUTPUTS]; |
pwensing | 0:43448bf056e8 | 121 | output_data[0] = t.read(); |
sangbae | 17:98e298577f09 | 122 | output_data[1] = th; |
sangbae | 17:98e298577f09 | 123 | output_data[2] = velocity; |
sangbae | 17:98e298577f09 | 124 | output_data[3] = current; |
sangbae | 17:98e298577f09 | 125 | output_data[4] = current_d; |
sangbae | 17:98e298577f09 | 126 | output_data[5] = volt; |
sangbae | 17:98e298577f09 | 127 | |
elijahsj | 13:3a1f4e09789b | 128 | |
pwensing | 0:43448bf056e8 | 129 | // Send data to MATLAB |
sangbae | 17:98e298577f09 | 130 | server.sendData(output_data,NUM_OUTPUTS); |
elijahsj | 13:3a1f4e09789b | 131 | wait(.001); //run control loop at 1kHz |
elijahsj | 4:7a1b35f081bb | 132 | } |
pwensing | 0:43448bf056e8 | 133 | // Cleanup after experiment |
sangbae | 17:98e298577f09 | 134 | ControlLoop.detach(); |
pwensing | 0:43448bf056e8 | 135 | server.setExperimentComplete(); |
elijahsj | 12:84a6dcb60422 | 136 | motorShield.motorAWrite(0, 0); //turn motor A off |
pwensing | 0:43448bf056e8 | 137 | } // end if |
pwensing | 0:43448bf056e8 | 138 | } // end while |
elijahsj | 10:a40d180c305c | 139 | |
elijahsj | 6:1faceb53dabe | 140 | } // end main |
elijahsj | 6:1faceb53dabe | 141 |