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@0:43448bf056e8, 2015-08-12 (annotated)

Committer:
pwensing
Date:
Wed Aug 12 11:54:33 2015 +0000
Revision:
0:43448bf056e8
Child:
1:95a7fddd25a9
initial commit

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
pwensing 0:43448bf056e8 7 #define NUM_INPUTS 2
pwensing 0:43448bf056e8 8 #define NUM_OUTPUTS 3
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
pwensing 0:43448bf056e8 12 PwmOut motorPMW(D5); // Motor PMW output
pwensing 0:43448bf056e8 13 DigitalOut motorFwd(D6); // Motor forward enable
pwensing 0:43448bf056e8 14 DigitalOut motorRev(D7); // Motor backward enable
pwensing 0:43448bf056e8 15 Timer t; // Timer to measure elapsed time of experiment
pwensing 0:43448bf056e8 16
pwensing 0:43448bf056e8 17 QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
pwensing 0:43448bf056e8 18
pwensing 0:43448bf056e8 19 int main (void) {
pwensing 0:43448bf056e8 20 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 21 server.attachTerminal(pc);
pwensing 0:43448bf056e8 22 server.init();
pwensing 0:43448bf056e8 23
pwensing 0:43448bf056e8 24 // PMW period should nominally be a multiple of our control loop
pwensing 0:43448bf056e8 25 motorPMW.period_us(500);
pwensing 0:43448bf056e8 26
pwensing 0:43448bf056e8 27 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 28 float input_params[NUM_INPUTS];
pwensing 0:43448bf056e8 29
pwensing 0:43448bf056e8 30 while(1) {
pwensing 0:43448bf056e8 31 if (server.getParams(input_params,NUM_INPUTS)) {
pwensing 0:43448bf056e8 32 float v1 = input_params[0]; // Voltage for first second
pwensing 0:43448bf056e8 33 float v2 = input_params[1]; // Voltage for second second
pwensing 0:43448bf056e8 34
pwensing 0:43448bf056e8 35 // Setup experiment
pwensing 0:43448bf056e8 36 t.reset();
pwensing 0:43448bf056e8 37 t.start();
pwensing 0:43448bf056e8 38 encoder.reset();
pwensing 0:43448bf056e8 39 motorFwd = 1;
pwensing 0:43448bf056e8 40 motorRev = 0;
pwensing 0:43448bf056e8 41 motorPMW.write(0);
pwensing 0:43448bf056e8 42
pwensing 0:43448bf056e8 43 // Run experiment
pwensing 0:43448bf056e8 44 while( t.read() < 2 ) {
pwensing 0:43448bf056e8 45 // Perform control loop logic
pwensing 0:43448bf056e8 46 if (t.read() < 1)
pwensing 0:43448bf056e8 47 motorPMW.write(v1);
pwensing 0:43448bf056e8 48 else
pwensing 0:43448bf056e8 49 motorPMW.write(v2);
pwensing 0:43448bf056e8 50
pwensing 0:43448bf056e8 51 // Form output to send to MATLAB
pwensing 0:43448bf056e8 52 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 53 output_data[0] = t.read();
pwensing 0:43448bf056e8 54 output_data[1] = encoder.getPulses();
pwensing 0:43448bf056e8 55 output_data[2] = encoder.getVelocity();
pwensing 0:43448bf056e8 56
pwensing 0:43448bf056e8 57 // Send data to MATLAB
pwensing 0:43448bf056e8 58 server.sendData(output_data,NUM_OUTPUTS);
pwensing 0:43448bf056e8 59 wait(.001);
pwensing 0:43448bf056e8 60 }
pwensing 0:43448bf056e8 61 // Cleanup after experiment
pwensing 0:43448bf056e8 62 server.setExperimentComplete();
pwensing 0:43448bf056e8 63 motorPMW.write(0);
pwensing 0:43448bf056e8 64 } // end if
pwensing 0:43448bf056e8 65 } // end while
pwensing 0:43448bf056e8 66 } // end main