Stephen Laskowski / Mbed 2 deprecated Dolphin

2.74 Bio-Inspired Robotics robot for LGO group. This is the vertical dolphin tail

Dependencies:   EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed

Fork of experiment_example by Patrick Wensing

main.cpp@3:c8e53b5762bd, 2015-10-03 (annotated)

Committer:
laskowsk
Date:
Sat Oct 03 19:42:23 2015 +0000
Revision:
3:c8e53b5762bd
Parent:
1:95a7fddd25a9
Child:
4:300ced917633
for Cragun

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"
pwensing 0:43448bf056e8 6
laskowsk 3:c8e53b5762bd 7 #define NUM_INPUTS 7
laskowsk 3:c8e53b5762bd 8 #define NUM_OUTPUTS 6
pwensing 0:43448bf056e8 9
pwensing 0:43448bf056e8 10 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 11 ExperimentServer server; // Object that lets us communicate with MATLAB
laskowsk 3:c8e53b5762bd 12 PwmOut motorPWM(D7); // Motor PWM output
laskowsk 3:c8e53b5762bd 13 DigitalOut motorFwd(D5); // Motor forward enable
laskowsk 3:c8e53b5762bd 14 DigitalOut motorRev(D6); // Motor backward enable
pwensing 0:43448bf056e8 15 Timer t; // Timer to measure elapsed time of experiment
laskowsk 3:c8e53b5762bd 16 Ticker k; // Time interval
laskowsk 3:c8e53b5762bd 17 AnalogIn Anal(A5); // AnalogIn port for current sensing
laskowsk 3:c8e53b5762bd 18 QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
laskowsk 3:c8e53b5762bd 19 float Volt = 0;
laskowsk 3:c8e53b5762bd 20 float emf = 0;
laskowsk 3:c8e53b5762bd 21 float Current =0;
laskowsk 3:c8e53b5762bd 22 float i = 0;
laskowsk 3:c8e53b5762bd 23 float P = 0;
laskowsk 3:c8e53b5762bd 24 float vel=0;
laskowsk 3:c8e53b5762bd 25 float pot=0;
laskowsk 3:c8e53b5762bd 26 float i_d=0;
pwensing 0:43448bf056e8 27
pwensing 0:43448bf056e8 28
pwensing 0:43448bf056e8 29 int main (void) {
pwensing 0:43448bf056e8 30 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 31 server.attachTerminal(pc);
pwensing 0:43448bf056e8 32 server.init();
pwensing 0:43448bf056e8 33
Patrick Wensing 1:95a7fddd25a9 34 // PWM period should nominally be a multiple of our control loop
laskowsk 3:c8e53b5762bd 35 motorPWM.period_us(200);
pwensing 0:43448bf056e8 36
pwensing 0:43448bf056e8 37 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 38 float input_params[NUM_INPUTS];
pwensing 0:43448bf056e8 39
pwensing 0:43448bf056e8 40 while(1) {
pwensing 0:43448bf056e8 41 if (server.getParams(input_params,NUM_INPUTS)) {
laskowsk 3:c8e53b5762bd 42 float tf = input_params[0]; // Voltage for first second
laskowsk 3:c8e53b5762bd 43 float i_d = input_params[1]; // Voltage for second second
laskowsk 3:c8e53b5762bd 44 float Kp = input_params[2];
laskowsk 3:c8e53b5762bd 45 float R = input_params[3];
laskowsk 3:c8e53b5762bd 46 float Kb = input_params[4];
laskowsk 3:c8e53b5762bd 47 float Kth = input_params[5];
laskowsk 3:c8e53b5762bd 48 float b = input_params[6];
laskowsk 3:c8e53b5762bd 49 //float .0thd = input_params[3];
laskowsk 3:c8e53b5762bd 50 Volt = 0.0;
laskowsk 3:c8e53b5762bd 51 emf = 0.0;
laskowsk 3:c8e53b5762bd 52 Current =0.0;
laskowsk 3:c8e53b5762bd 53 i = 0.0;
laskowsk 3:c8e53b5762bd 54 P = 0.0;
laskowsk 3:c8e53b5762bd 55 vel=0.0;
laskowsk 3:c8e53b5762bd 56 pot=0.0;
laskowsk 3:c8e53b5762bd 57 i_d=0.0;
pwensing 0:43448bf056e8 58 // Setup experiment
pwensing 0:43448bf056e8 59 t.reset();
pwensing 0:43448bf056e8 60 t.start();
pwensing 0:43448bf056e8 61 encoder.reset();
laskowsk 3:c8e53b5762bd 62 //float thj = 0;
pwensing 0:43448bf056e8 63 motorFwd = 1;
pwensing 0:43448bf056e8 64 motorRev = 0;
Patrick Wensing 1:95a7fddd25a9 65 motorPWM.write(0);
pwensing 0:43448bf056e8 66
pwensing 0:43448bf056e8 67 // Run experiment
laskowsk 3:c8e53b5762bd 68 while( t.read() < tf ) {
pwensing 0:43448bf056e8 69 // Perform control loop logic
laskowsk 3:c8e53b5762bd 70 //void attime(){
laskowsk 3:c8e53b5762bd 71 //motorPWM.write(v1);
laskowsk 3:c8e53b5762bd 72
laskowsk 3:c8e53b5762bd 73
laskowsk 3:c8e53b5762bd 74 //k.attach_us(0, 2000);
laskowsk 3:c8e53b5762bd 75 // This is for position based PID
laskowsk 3:c8e53b5762bd 76 //thj = thj + (thd - encoder.getPulses());
laskowsk 3:c8e53b5762bd 77 //float P= Kp*(thd-encoder.getPulses());
laskowsk 3:c8e53b5762bd 78 //float I= Ki*(0-encoder.getVelocity());
laskowsk 3:c8e53b5762bd 79 //float D= Kd*thj;
laskowsk 3:c8e53b5762bd 80 // float Volt = P+I+D;
laskowsk 3:c8e53b5762bd 81 // float pulse = encoder.getPulses();
laskowsk 3:c8e53b5762bd 82
laskowsk 3:c8e53b5762bd 83
laskowsk 3:c8e53b5762bd 84 //below is for Current Based PID
laskowsk 3:c8e53b5762bd 85 Current = Anal.read();
laskowsk 3:c8e53b5762bd 86 vel=encoder.getVelocity()/1200.0*6.28;
laskowsk 3:c8e53b5762bd 87 pot=encoder.getPulses()/1200.0*6.28;
laskowsk 3:c8e53b5762bd 88 i_d=(Kth*pot+b*vel)/Kb;
laskowsk 3:c8e53b5762bd 89 //i_d = 1.5;
laskowsk 3:c8e53b5762bd 90 i = 3.3f*Current/0.140f;
laskowsk 3:c8e53b5762bd 91 if (i_d<0){
laskowsk 3:c8e53b5762bd 92 i=i*-1;
laskowsk 3:c8e53b5762bd 93 }
laskowsk 3:c8e53b5762bd 94 P = Kp*(i_d-i);
laskowsk 3:c8e53b5762bd 95 emf = Kb*vel;
laskowsk 3:c8e53b5762bd 96 Volt = (R*i_d+P+emf)/12.0;
laskowsk 3:c8e53b5762bd 97 //float Kb=(Volt-R*i_d)/vel;
laskowsk 3:c8e53b5762bd 98
laskowsk 3:c8e53b5762bd 99 //float Volt = 0;
laskowsk 3:c8e53b5762bd 100 //pc.printf(" %f /n",P);
laskowsk 3:c8e53b5762bd 101 if (Volt>0){
laskowsk 3:c8e53b5762bd 102 motorRev.write(1);
laskowsk 3:c8e53b5762bd 103 motorFwd.write(0);
laskowsk 3:c8e53b5762bd 104 motorPWM.write(Volt);
laskowsk 3:c8e53b5762bd 105 }
laskowsk 3:c8e53b5762bd 106 else{
laskowsk 3:c8e53b5762bd 107 motorFwd.write(1);
laskowsk 3:c8e53b5762bd 108 motorRev.write(0);
laskowsk 3:c8e53b5762bd 109 motorPWM.write(-1*Volt);
laskowsk 3:c8e53b5762bd 110 }
laskowsk 3:c8e53b5762bd 111
laskowsk 3:c8e53b5762bd 112 //if (t.read() < 1)
laskowsk 3:c8e53b5762bd 113 //motorPWM.write(v1);
laskowsk 3:c8e53b5762bd 114 //else
laskowsk 3:c8e53b5762bd 115 // motorPWM.write(v2);
pwensing 0:43448bf056e8 116
pwensing 0:43448bf056e8 117 // Form output to send to MATLAB
pwensing 0:43448bf056e8 118 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 119 output_data[0] = t.read();
laskowsk 3:c8e53b5762bd 120 output_data[1] = encoder.getPulses()/1200.0*6.28;
laskowsk 3:c8e53b5762bd 121 output_data[2] = vel;
laskowsk 3:c8e53b5762bd 122 output_data[3] = Volt;
laskowsk 3:c8e53b5762bd 123 output_data[4] = i;
laskowsk 3:c8e53b5762bd 124 output_data[5] = i_d;
pwensing 0:43448bf056e8 125
pwensing 0:43448bf056e8 126 // Send data to MATLAB
pwensing 0:43448bf056e8 127 server.sendData(output_data,NUM_OUTPUTS);
pwensing 0:43448bf056e8 128 wait(.001);
pwensing 0:43448bf056e8 129 }
pwensing 0:43448bf056e8 130 // Cleanup after experiment
pwensing 0:43448bf056e8 131 server.setExperimentComplete();
Patrick Wensing 1:95a7fddd25a9 132 motorPWM.write(0);
pwensing 0:43448bf056e8 133 } // end if
pwensing 0:43448bf056e8 134 } // end while
laskowsk 3:c8e53b5762bd 135
laskowsk 3:c8e53b5762bd 136 }// end main