sangbae Kim / Mbed OS Lab3_experiment_example

This is a good example code for 2.74 lab 3 impedance control

Dependencies:   ExperimentServer QEI_pmw MotorShield

main.cpp@20:4c172a0737c6, 2020-09-26 (annotated)

Committer:
sangbae
Date:
Sat Sep 26 00:19:16 2020 +0000
Revision:
20:4c172a0737c6
Parent:
19:d58bf34141e9 
ok

Who changed what in which revision?

UserRevisionLine numberNew 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 20:4c172a0737c6 27 Ticker ControlLoop; //Ticker object created
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 20:4c172a0737c6 94 ControlLoop.attach(&current_control,period); //start current loop using Ticker function
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