Sridevi Kaza / Mbed OS project

nov 18th

Dependencies:   Bezier_Traj_Follower_Example ExperimentServer QEI_pmw MotorShield

main.cpp@10:a40d180c305c, 2020-08-26 (annotated)

Committer:
elijahsj
Date:
Wed Aug 26 02:44:03 2020 +0000
Revision:
10:a40d180c305c
Parent:
9:97360c92666f
Child:
11:4eb579687dda
updated for shield

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 8:98a83981c238 7 #include "HardwareSetup.h"
pwensing 0:43448bf056e8 8
pwensing 0:43448bf056e8 9 #define NUM_INPUTS 2
pwensing 0:43448bf056e8 10 #define NUM_OUTPUTS 3
pwensing 0:43448bf056e8 11
pwensing 0:43448bf056e8 12 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 13 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 14 Timer t; // Timer to measure elapsed time of experiment
elijahsj 5:1ab9b2527794 15
elijahsj 5:1ab9b2527794 16 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 17 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 18 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 19 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 20
elijahsj 6:1faceb53dabe 21 AnalogIn currentA(PF_12); //MOTOR A CURRENT SENSOR
elijahsj 6:1faceb53dabe 22 AnalogIn currentB(PF_11); //MOTOR B CURRENT SENSOR
elijahsj 6:1faceb53dabe 23 AnalogIn currentC(PF_13); //MOTOR C CURRENT SENSOR
elijahsj 6:1faceb53dabe 24 AnalogIn currentD(PA_4); //MOTOR D CURRENT SENSOR
elijahsj 6:1faceb53dabe 25
elijahsj 10:a40d180c305c 26 MotorShield motors(1000); //initialize the motor shield with a period of 1000 ticks or xxx kHZ
elijahsj 6:1faceb53dabe 27
elijahsj 4:7a1b35f081bb 28 int main (void)
elijahsj 4:7a1b35f081bb 29 {
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
pwensing 0:43448bf056e8 34 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 35 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 36 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 37
pwensing 0:43448bf056e8 38 while(1) {
pwensing 0:43448bf056e8 39 if (server.getParams(input_params,NUM_INPUTS)) {
pwensing 0:43448bf056e8 40 float v1 = input_params[0]; // Voltage for first second
pwensing 0:43448bf056e8 41 float v2 = input_params[1]; // Voltage for second second
elijahsj 4:7a1b35f081bb 42
pwensing 0:43448bf056e8 43 // Setup experiment
pwensing 0:43448bf056e8 44 t.reset();
pwensing 0:43448bf056e8 45 t.start();
elijahsj 5:1ab9b2527794 46 encoderA.reset();
elijahsj 5:1ab9b2527794 47 encoderB.reset();
elijahsj 5:1ab9b2527794 48 encoderC.reset();
elijahsj 5:1ab9b2527794 49 encoderD.reset();
elijahsj 10:a40d180c305c 50
elijahsj 10:a40d180c305c 51 motors.motorAWrite(0, 0); //turn motor A off
pwensing 0:43448bf056e8 52
pwensing 0:43448bf056e8 53 // Run experiment
elijahsj 4:7a1b35f081bb 54 while( t.read() < 2 ) {
pwensing 0:43448bf056e8 55 // Perform control loop logic
elijahsj 4:7a1b35f081bb 56 if (t.read() < 1)
elijahsj 10:a40d180c305c 57 motors.motorAWrite(v1, 0); //run motor A at "v1" duty cycle and in the forward direction
elijahsj 4:7a1b35f081bb 58 else
elijahsj 10:a40d180c305c 59 motors.motorAWrite(v2, 0); //run motor A at "v2" duty cycle and in the forward direction
elijahsj 4:7a1b35f081bb 60
elijahsj 4:7a1b35f081bb 61 // Form output to send to MATLAB
pwensing 0:43448bf056e8 62 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 63 output_data[0] = t.read();
elijahsj 5:1ab9b2527794 64 output_data[1] = encoderA.getPulses();
elijahsj 5:1ab9b2527794 65 output_data[2] = encoderA.getVelocity();
elijahsj 4:7a1b35f081bb 66
pwensing 0:43448bf056e8 67 // Send data to MATLAB
pwensing 0:43448bf056e8 68 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 4:7a1b35f081bb 69 wait(.001);
elijahsj 4:7a1b35f081bb 70 }
pwensing 0:43448bf056e8 71 // Cleanup after experiment
pwensing 0:43448bf056e8 72 server.setExperimentComplete();
elijahsj 10:a40d180c305c 73 motors.motorAWrite(0, 0); //turn motor A off
pwensing 0:43448bf056e8 74 } // end if
pwensing 0:43448bf056e8 75 } // end while
elijahsj 10:a40d180c305c 76
elijahsj 6:1faceb53dabe 77 } // end main
elijahsj 6:1faceb53dabe 78