Elijah Stanger-Jones / Mbed OS Lab3_experiment_example

Modified experiment example for lab 3 of 2.74

Dependencies:   ExperimentServer MotorShield QEI_pmw

main.cpp@17:98e298577f09, 2020-09-25 (annotated)

Committer:
sangbae
Date:
Fri Sep 25 01:07:22 2020 +0000
Revision:
17:98e298577f09
Parent:
16:bbe4ac38c535
Child:
18:54195aa5e534
This is 2.74 Lab3 example code for impedance control.

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