B M / Mbed OS final_project23

first

Dependencies:   ExperimentServer QEI_pmw MotorShield

main.cpp@5:1ab9b2527794, 2020-08-21 (annotated)

Committer:
elijahsj
Date:
Fri Aug 21 22:13:19 2020 +0000
Revision:
5:1ab9b2527794
Parent:
4:7a1b35f081bb
Child:
6:1faceb53dabe
Fixed encoder stuff

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
elijahsj 5:1ab9b2527794 12 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 13
elijahsj 5:1ab9b2527794 14 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 15 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 16 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 17 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 18
Patrick Wensing 1:95a7fddd25a9 19 PwmOut motorPWM(D5); // Motor PWM output
pwensing 0:43448bf056e8 20 DigitalOut motorFwd(D6); // Motor forward enable
pwensing 0:43448bf056e8 21 DigitalOut motorRev(D7); // Motor backward enable
pwensing 0:43448bf056e8 22
elijahsj 4:7a1b35f081bb 23 int main (void)
elijahsj 4:7a1b35f081bb 24 {
pwensing 0:43448bf056e8 25 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 26 server.attachTerminal(pc);
pwensing 0:43448bf056e8 27 server.init();
pwensing 0:43448bf056e8 28
Patrick Wensing 1:95a7fddd25a9 29 // PWM period should nominally be a multiple of our control loop
Patrick Wensing 1:95a7fddd25a9 30 motorPWM.period_us(500);
elijahsj 4:7a1b35f081bb 31
pwensing 0:43448bf056e8 32 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 33 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 34 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 35
pwensing 0:43448bf056e8 36 while(1) {
pwensing 0:43448bf056e8 37 if (server.getParams(input_params,NUM_INPUTS)) {
pwensing 0:43448bf056e8 38 float v1 = input_params[0]; // Voltage for first second
pwensing 0:43448bf056e8 39 float v2 = input_params[1]; // Voltage for second second
elijahsj 4:7a1b35f081bb 40
pwensing 0:43448bf056e8 41 // Setup experiment
pwensing 0:43448bf056e8 42 t.reset();
pwensing 0:43448bf056e8 43 t.start();
elijahsj 5:1ab9b2527794 44 encoderA.reset();
elijahsj 5:1ab9b2527794 45 encoderB.reset();
elijahsj 5:1ab9b2527794 46 encoderC.reset();
elijahsj 5:1ab9b2527794 47 encoderD.reset();
pwensing 0:43448bf056e8 48 motorFwd = 1;
pwensing 0:43448bf056e8 49 motorRev = 0;
Patrick Wensing 1:95a7fddd25a9 50 motorPWM.write(0);
pwensing 0:43448bf056e8 51
pwensing 0:43448bf056e8 52 // Run experiment
elijahsj 4:7a1b35f081bb 53 while( t.read() < 2 ) {
pwensing 0:43448bf056e8 54 // Perform control loop logic
elijahsj 4:7a1b35f081bb 55 if (t.read() < 1)
Patrick Wensing 1:95a7fddd25a9 56 motorPWM.write(v1);
elijahsj 4:7a1b35f081bb 57 else
Patrick Wensing 1:95a7fddd25a9 58 motorPWM.write(v2);
elijahsj 4:7a1b35f081bb 59
elijahsj 4:7a1b35f081bb 60 // Form output to send to MATLAB
pwensing 0:43448bf056e8 61 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 62 output_data[0] = t.read();
elijahsj 5:1ab9b2527794 63 output_data[1] = encoderA.getPulses();
elijahsj 5:1ab9b2527794 64 output_data[2] = encoderA.getVelocity();
elijahsj 4:7a1b35f081bb 65
pwensing 0:43448bf056e8 66 // Send data to MATLAB
pwensing 0:43448bf056e8 67 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 4:7a1b35f081bb 68 wait(.001);
elijahsj 4:7a1b35f081bb 69 }
pwensing 0:43448bf056e8 70 // Cleanup after experiment
pwensing 0:43448bf056e8 71 server.setExperimentComplete();
Patrick Wensing 1:95a7fddd25a9 72 motorPWM.write(0);
pwensing 0:43448bf056e8 73 } // end if
pwensing 0:43448bf056e8 74 } // end while
pwensing 0:43448bf056e8 75 } // end main