Sridevi Kaza / Mbed OS project

nov 18th

Dependencies:   Bezier_Traj_Follower_Example ExperimentServer QEI_pmw MotorShield

main.cpp@6:1faceb53dabe, 2020-08-24 (annotated)

Committer:
elijahsj
Date:
Mon Aug 24 20:19:13 2020 +0000
Revision:
6:1faceb53dabe
Parent:
5:1ab9b2527794
Child:
8:98a83981c238
Added motorshield ;

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"
pwensing 0:43448bf056e8 7
pwensing 0:43448bf056e8 8 #define NUM_INPUTS 2
pwensing 0:43448bf056e8 9 #define NUM_OUTPUTS 3
pwensing 0:43448bf056e8 10
pwensing 0:43448bf056e8 11 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 12 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 13 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 14
elijahsj 5:1ab9b2527794 15 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 16 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 17 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 18 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 19
elijahsj 6:1faceb53dabe 20 AnalogIn currentA(PF_12); //MOTOR A CURRENT SENSOR
elijahsj 6:1faceb53dabe 21 AnalogIn currentB(PF_11); //MOTOR B CURRENT SENSOR
elijahsj 6:1faceb53dabe 22 AnalogIn currentC(PF_13); //MOTOR C CURRENT SENSOR
elijahsj 6:1faceb53dabe 23 AnalogIn currentD(PA_4); //MOTOR D CURRENT SENSOR
elijahsj 6:1faceb53dabe 24
elijahsj 6:1faceb53dabe 25 MotorShield motorA(PE_5, PE_6); //MOTOR A PWM
elijahsj 6:1faceb53dabe 26 MotorShield motorB(PB_14, PB_15); //MOTOR B PWM
elijahsj 6:1faceb53dabe 27 MotorShield motorC(PA_6, PF_9); //MOTOR C PWM
elijahsj 6:1faceb53dabe 28 MotorShield motorD(PF_6, PF_7); //MOTOR D PWM
elijahsj 6:1faceb53dabe 29
elijahsj 6:1faceb53dabe 30 //deprecate the following
Patrick Wensing 1:95a7fddd25a9 31 PwmOut motorPWM(D5); // Motor PWM output
pwensing 0:43448bf056e8 32 DigitalOut motorFwd(D6); // Motor forward enable
pwensing 0:43448bf056e8 33 DigitalOut motorRev(D7); // Motor backward enable
pwensing 0:43448bf056e8 34
elijahsj 6:1faceb53dabe 35
elijahsj 6:1faceb53dabe 36
elijahsj 4:7a1b35f081bb 37 int main (void)
elijahsj 4:7a1b35f081bb 38 {
pwensing 0:43448bf056e8 39 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 40 server.attachTerminal(pc);
pwensing 0:43448bf056e8 41 server.init();
pwensing 0:43448bf056e8 42
Patrick Wensing 1:95a7fddd25a9 43 // PWM period should nominally be a multiple of our control loop
Patrick Wensing 1:95a7fddd25a9 44 motorPWM.period_us(500);
elijahsj 4:7a1b35f081bb 45
pwensing 0:43448bf056e8 46 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 47 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 48 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 49
pwensing 0:43448bf056e8 50 while(1) {
pwensing 0:43448bf056e8 51 if (server.getParams(input_params,NUM_INPUTS)) {
pwensing 0:43448bf056e8 52 float v1 = input_params[0]; // Voltage for first second
pwensing 0:43448bf056e8 53 float v2 = input_params[1]; // Voltage for second second
elijahsj 4:7a1b35f081bb 54
pwensing 0:43448bf056e8 55 // Setup experiment
pwensing 0:43448bf056e8 56 t.reset();
pwensing 0:43448bf056e8 57 t.start();
elijahsj 5:1ab9b2527794 58 encoderA.reset();
elijahsj 5:1ab9b2527794 59 encoderB.reset();
elijahsj 5:1ab9b2527794 60 encoderC.reset();
elijahsj 5:1ab9b2527794 61 encoderD.reset();
pwensing 0:43448bf056e8 62 motorFwd = 1;
pwensing 0:43448bf056e8 63 motorRev = 0;
Patrick Wensing 1:95a7fddd25a9 64 motorPWM.write(0);
pwensing 0:43448bf056e8 65
pwensing 0:43448bf056e8 66 // Run experiment
elijahsj 4:7a1b35f081bb 67 while( t.read() < 2 ) {
pwensing 0:43448bf056e8 68 // Perform control loop logic
elijahsj 4:7a1b35f081bb 69 if (t.read() < 1)
Patrick Wensing 1:95a7fddd25a9 70 motorPWM.write(v1);
elijahsj 4:7a1b35f081bb 71 else
Patrick Wensing 1:95a7fddd25a9 72 motorPWM.write(v2);
elijahsj 4:7a1b35f081bb 73
elijahsj 4:7a1b35f081bb 74 // Form output to send to MATLAB
pwensing 0:43448bf056e8 75 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 76 output_data[0] = t.read();
elijahsj 5:1ab9b2527794 77 output_data[1] = encoderA.getPulses();
elijahsj 5:1ab9b2527794 78 output_data[2] = encoderA.getVelocity();
elijahsj 4:7a1b35f081bb 79
pwensing 0:43448bf056e8 80 // Send data to MATLAB
pwensing 0:43448bf056e8 81 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 4:7a1b35f081bb 82 wait(.001);
elijahsj 4:7a1b35f081bb 83 }
pwensing 0:43448bf056e8 84 // Cleanup after experiment
pwensing 0:43448bf056e8 85 server.setExperimentComplete();
Patrick Wensing 1:95a7fddd25a9 86 motorPWM.write(0);
pwensing 0:43448bf056e8 87 } // end if
pwensing 0:43448bf056e8 88 } // end while
elijahsj 6:1faceb53dabe 89 } // end main
elijahsj 6:1faceb53dabe 90