Morgan Mayborne / Mbed OS Lab3_experiment_example

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ExperimentServer QEI_pmw MotorShield

main.cpp@21:94aeb8abe04c, 2021-09-25 (annotated)

Committer:
elijahsj
Date:
Sat Sep 25 21:05:54 2021 +0000
Revision:
21:94aeb8abe04c
Parent:
20:4c172a0737c6
Child:
22:c010476ffeb8
Fixed formatting

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
elijahsj 21:94aeb8abe04c 9 #define NUM_INPUTS 3
elijahsj 21:94aeb8abe04c 10 #define NUM_OUTPUTS 5
elijahsj 21:94aeb8abe04c 11
elijahsj 21:94aeb8abe04c 12 //Measured values
sangbae 17:98e298577f09 13 float velocity = 0;
sangbae 17:98e298577f09 14 float current = 0;
elijahsj 21:94aeb8abe04c 15 float theta = 0;
elijahsj 21:94aeb8abe04c 16
elijahsj 21:94aeb8abe04c 17 //Set values
sangbae 17:98e298577f09 18 float current_d = 0;
elijahsj 21:94aeb8abe04c 19 float kp = 0;
elijahsj 21:94aeb8abe04c 20 float ki = 0;
elijahsj 21:94aeb8abe04c 21
elijahsj 21:94aeb8abe04c 22 //Controller values
sangbae 17:98e298577f09 23 float volt = 0;
sangbae 17:98e298577f09 24 float duty = 0;
pwensing 0:43448bf056e8 25
pwensing 0:43448bf056e8 26 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 27 ExperimentServer server; // Object that lets us communicate with MATLAB
elijahsj 5:1ab9b2527794 28 Timer t; // Timer to measure elapsed time of experiment
sangbae 20:4c172a0737c6 29 Ticker ControlLoop; //Ticker object created
elijahsj 5:1ab9b2527794 30
elijahsj 5:1ab9b2527794 31 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 32 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 33 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 34 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
elijahsj 5:1ab9b2527794 35
elijahsj 16:bbe4ac38c535 36 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ
elijahsj 6:1faceb53dabe 37
sangbae 19:d58bf34141e9 38 void current_control() //Current control function
sangbae 17:98e298577f09 39 {
sangbae 17:98e298577f09 40 float error = 0;
sangbae 17:98e298577f09 41 velocity = encoderA.getVelocity()*(6.2831/1200.0);
sangbae 17:98e298577f09 42 current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.15); //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab.
sangbae 17:98e298577f09 43 error = current_d - current;
elijahsj 21:94aeb8abe04c 44
elijahsj 21:94aeb8abe04c 45 volt = 0; //CHANGE THIS TO SET CURRENT CONTROLLER VALUES
elijahsj 21:94aeb8abe04c 46
sangbae 17:98e298577f09 47 duty = volt/12.0;
sangbae 17:98e298577f09 48 if (duty > 1){
sangbae 17:98e298577f09 49 duty = 1;
sangbae 17:98e298577f09 50 }
sangbae 17:98e298577f09 51 if (duty <-1){
sangbae 17:98e298577f09 52 duty = -1;
sangbae 17:98e298577f09 53 }
sangbae 17:98e298577f09 54 if (duty >= 0){
sangbae 17:98e298577f09 55 motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction
sangbae 17:98e298577f09 56 }
sangbae 17:98e298577f09 57 else if (duty < 0){
sangbae 17:98e298577f09 58 motorShield.motorAWrite(abs(duty), 1);
sangbae 17:98e298577f09 59 }
sangbae 17:98e298577f09 60 }
sangbae 17:98e298577f09 61
sangbae 17:98e298577f09 62
elijahsj 4:7a1b35f081bb 63 int main (void)
elijahsj 4:7a1b35f081bb 64 {
pwensing 0:43448bf056e8 65 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 66 server.attachTerminal(pc);
pwensing 0:43448bf056e8 67 server.init();
elijahsj 13:3a1f4e09789b 68
pwensing 0:43448bf056e8 69 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 70 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 71 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 72
pwensing 0:43448bf056e8 73 while(1) {
pwensing 0:43448bf056e8 74 if (server.getParams(input_params,NUM_INPUTS)) {
elijahsj 21:94aeb8abe04c 75 kp = input_params[0];
elijahsj 21:94aeb8abe04c 76 ki = input_params[1];
elijahsj 21:94aeb8abe04c 77 current_d = input_params[2];
elijahsj 21:94aeb8abe04c 78
elijahsj 21:94aeb8abe04c 79 ControlLoop.attach(&current_control,0.0001); //Run current controller at 10khz
sangbae 17:98e298577f09 80
pwensing 0:43448bf056e8 81 // Setup experiment
pwensing 0:43448bf056e8 82 t.reset();
pwensing 0:43448bf056e8 83 t.start();
elijahsj 5:1ab9b2527794 84 encoderA.reset();
elijahsj 5:1ab9b2527794 85 encoderB.reset();
elijahsj 5:1ab9b2527794 86 encoderC.reset();
elijahsj 5:1ab9b2527794 87 encoderD.reset();
elijahsj 10:a40d180c305c 88
elijahsj 15:495333b2ccf1 89 motorShield.motorAWrite(0, 0); //turn motor A off
elijahsj 15:495333b2ccf1 90
elijahsj 15:495333b2ccf1 91 //use the motor shield as follows:
elijahsj 15:495333b2ccf1 92 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
elijahsj 15:495333b2ccf1 93
pwensing 0:43448bf056e8 94 // Run experiment
sangbae 17:98e298577f09 95 while( t.read() < 5 ) {
pwensing 0:43448bf056e8 96 // Perform control loop logic
elijahsj 21:94aeb8abe04c 97 // current_d = 0; // Set commanded current from impedance controller here.
sangbae 17:98e298577f09 98
elijahsj 21:94aeb8abe04c 99 // Send data to MATLAB
pwensing 0:43448bf056e8 100 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 101 output_data[0] = t.read();
elijahsj 21:94aeb8abe04c 102 output_data[1] = theta;
sangbae 17:98e298577f09 103 output_data[2] = velocity;
sangbae 17:98e298577f09 104 output_data[3] = current;
elijahsj 21:94aeb8abe04c 105 output_data[4] = volt;
elijahsj 21:94aeb8abe04c 106
sangbae 17:98e298577f09 107 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 21:94aeb8abe04c 108 wait(.001); //run outer control loop at 1kHz
elijahsj 4:7a1b35f081bb 109 }
pwensing 0:43448bf056e8 110 // Cleanup after experiment
sangbae 17:98e298577f09 111 ControlLoop.detach();
pwensing 0:43448bf056e8 112 server.setExperimentComplete();
elijahsj 12:84a6dcb60422 113 motorShield.motorAWrite(0, 0); //turn motor A off
pwensing 0:43448bf056e8 114 } // end if
pwensing 0:43448bf056e8 115 } // end while
elijahsj 10:a40d180c305c 116
elijahsj 6:1faceb53dabe 117 } // end main
elijahsj 6:1faceb53dabe 118