Aditya Mehrotra / Mbed OS experiment_example

2.74 Experiment Example Code (updated)

Dependencies:   ExperimentServer MotorShield QEI_pmw

main.cpp@17:1fc3325b8b2e, 2022-09-16 (annotated)

Committer:
adimmit
Date:
Fri Sep 16 01:39:18 2022 +0000
Revision:
17:1fc3325b8b2e
Parent:
16:bbe4ac38c535 
updated for new motor driver;

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
pwensing 0:43448bf056e8 9 #define NUM_INPUTS 2
elijahsj 13:3a1f4e09789b 10 #define NUM_OUTPUTS 4
elijahsj 13:3a1f4e09789b 11
pwensing 0:43448bf056e8 12
pwensing 0:43448bf056e8 13 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 14 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 15 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 16
elijahsj 5:1ab9b2527794 17 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 18 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 19 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 20 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 21
elijahsj 16:bbe4ac38c535 22 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ
elijahsj 6:1faceb53dabe 23
elijahsj 4:7a1b35f081bb 24 int main (void)
elijahsj 4:7a1b35f081bb 25 {
pwensing 0:43448bf056e8 26 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 27 server.attachTerminal(pc);
pwensing 0:43448bf056e8 28 server.init();
elijahsj 13:3a1f4e09789b 29
pwensing 0:43448bf056e8 30 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 31 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 32 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 33
pwensing 0:43448bf056e8 34 while(1) {
pwensing 0:43448bf056e8 35 if (server.getParams(input_params,NUM_INPUTS)) {
elijahsj 16:bbe4ac38c535 36 float d1 = input_params[0]; // Duty cycle for first second
elijahsj 16:bbe4ac38c535 37 float d2 = input_params[1]; // Duty cycle for second second
elijahsj 4:7a1b35f081bb 38
pwensing 0:43448bf056e8 39 // Setup experiment
pwensing 0:43448bf056e8 40 t.reset();
pwensing 0:43448bf056e8 41 t.start();
elijahsj 5:1ab9b2527794 42 encoderA.reset();
elijahsj 5:1ab9b2527794 43 encoderB.reset();
elijahsj 5:1ab9b2527794 44 encoderC.reset();
elijahsj 5:1ab9b2527794 45 encoderD.reset();
elijahsj 10:a40d180c305c 46
elijahsj 15:495333b2ccf1 47 motorShield.motorAWrite(0, 0); //turn motor A off
elijahsj 15:495333b2ccf1 48
elijahsj 15:495333b2ccf1 49 //use the motor shield as follows:
elijahsj 15:495333b2ccf1 50 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
elijahsj 15:495333b2ccf1 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)
elijahsj 16:bbe4ac38c535 56 motorShield.motorAWrite(d1, 0); //run motor A at "v1" duty cycle and in the forward direction
elijahsj 4:7a1b35f081bb 57 else
elijahsj 16:bbe4ac38c535 58 motorShield.motorAWrite(d2, 0); //run motor A at "v2" duty cycle and in the forward direction
elijahsj 4:7a1b35f081bb 59
elijahsj 4:7a1b35f081bb 60 // Form output to send to MATLAB
pwensing 0:43448bf056e8 61 float output_data[NUM_OUTPUTS];
elijahsj 13:3a1f4e09789b 62
pwensing 0:43448bf056e8 63 output_data[0] = t.read();
elijahsj 16:bbe4ac38c535 64 output_data[1] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
elijahsj 16:bbe4ac38c535 65 output_data[2] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
elijahsj 16:bbe4ac38c535 66 output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
elijahsj 13:3a1f4e09789b 67
pwensing 0:43448bf056e8 68 // Send data to MATLAB
pwensing 0:43448bf056e8 69 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 13:3a1f4e09789b 70
elijahsj 13:3a1f4e09789b 71 wait(.001); //run control loop at 1kHz
elijahsj 4:7a1b35f081bb 72 }
pwensing 0:43448bf056e8 73 // Cleanup after experiment
pwensing 0:43448bf056e8 74 server.setExperimentComplete();
elijahsj 12:84a6dcb60422 75 motorShield.motorAWrite(0, 0); //turn motor A off
pwensing 0:43448bf056e8 76 } // end if
pwensing 0:43448bf056e8 77 } // end while
elijahsj 10:a40d180c305c 78
elijahsj 6:1faceb53dabe 79 } // end main
elijahsj 6:1faceb53dabe 80