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@24:4d2f1b7746de, 2021-09-27 (annotated)

Committer:
elijahsj
Date:
Mon Sep 27 19:55:59 2021 +0000
Revision:
24:4d2f1b7746de
Parent:
23:4056ae374b62
Child:
25:f5eaea12412f
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;
elijahsj 23:4056ae374b62 41 theta = encoderA.getPulses()*(6.2831/1200.0);
sangbae 17:98e298577f09 42 velocity = encoderA.getVelocity()*(6.2831/1200.0);
elijahsj 22:c010476ffeb8 43 current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.0); //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 44 error = current_d - current;
elijahsj 21:94aeb8abe04c 45
elijahsj 21:94aeb8abe04c 46 volt = 0; //CHANGE THIS TO SET CURRENT CONTROLLER VALUES
elijahsj 21:94aeb8abe04c 47
sangbae 17:98e298577f09 48 duty = volt/12.0;
sangbae 17:98e298577f09 49 if (duty > 1){
sangbae 17:98e298577f09 50 duty = 1;
sangbae 17:98e298577f09 51 }
sangbae 17:98e298577f09 52 if (duty <-1){
sangbae 17:98e298577f09 53 duty = -1;
sangbae 17:98e298577f09 54 }
sangbae 17:98e298577f09 55 if (duty >= 0){
sangbae 17:98e298577f09 56 motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction
sangbae 17:98e298577f09 57 }
sangbae 17:98e298577f09 58 else if (duty < 0){
sangbae 17:98e298577f09 59 motorShield.motorAWrite(abs(duty), 1);
sangbae 17:98e298577f09 60 }
sangbae 17:98e298577f09 61 }
sangbae 17:98e298577f09 62
sangbae 17:98e298577f09 63
elijahsj 4:7a1b35f081bb 64 int main (void)
elijahsj 4:7a1b35f081bb 65 {
pwensing 0:43448bf056e8 66 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 67 server.attachTerminal(pc);
pwensing 0:43448bf056e8 68 server.init();
elijahsj 13:3a1f4e09789b 69
pwensing 0:43448bf056e8 70 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 71 float input_params[NUM_INPUTS];
elijahsj 5:1ab9b2527794 72 pc.printf("%f",input_params[0]);
elijahsj 5:1ab9b2527794 73
pwensing 0:43448bf056e8 74 while(1) {
pwensing 0:43448bf056e8 75 if (server.getParams(input_params,NUM_INPUTS)) {
elijahsj 21:94aeb8abe04c 76 kp = input_params[0];
elijahsj 21:94aeb8abe04c 77 ki = input_params[1];
elijahsj 21:94aeb8abe04c 78 current_d = input_params[2];
elijahsj 21:94aeb8abe04c 79
elijahsj 21:94aeb8abe04c 80 ControlLoop.attach(&current_control,0.0001); //Run current controller at 10khz
sangbae 17:98e298577f09 81
pwensing 0:43448bf056e8 82 // Setup experiment
pwensing 0:43448bf056e8 83 t.reset();
pwensing 0:43448bf056e8 84 t.start();
elijahsj 5:1ab9b2527794 85 encoderA.reset();
elijahsj 5:1ab9b2527794 86 encoderB.reset();
elijahsj 5:1ab9b2527794 87 encoderC.reset();
elijahsj 5:1ab9b2527794 88 encoderD.reset();
elijahsj 10:a40d180c305c 89
elijahsj 15:495333b2ccf1 90 motorShield.motorAWrite(0, 0); //turn motor A off
elijahsj 15:495333b2ccf1 91
elijahsj 15:495333b2ccf1 92 //use the motor shield as follows:
elijahsj 15:495333b2ccf1 93 //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
elijahsj 15:495333b2ccf1 94
pwensing 0:43448bf056e8 95 // Run experiment
sangbae 17:98e298577f09 96 while( t.read() < 5 ) {
pwensing 0:43448bf056e8 97 // Perform control loop logic
elijahsj 23:4056ae374b62 98 //current_d = 0; // Set commanded current from impedance controller here.
sangbae 17:98e298577f09 99
elijahsj 21:94aeb8abe04c 100 // Send data to MATLAB
pwensing 0:43448bf056e8 101 float output_data[NUM_OUTPUTS];
pwensing 0:43448bf056e8 102 output_data[0] = t.read();
elijahsj 21:94aeb8abe04c 103 output_data[1] = theta;
sangbae 17:98e298577f09 104 output_data[2] = velocity;
sangbae 17:98e298577f09 105 output_data[3] = current;
elijahsj 21:94aeb8abe04c 106 output_data[4] = volt;
elijahsj 21:94aeb8abe04c 107
sangbae 17:98e298577f09 108 server.sendData(output_data,NUM_OUTPUTS);
elijahsj 21:94aeb8abe04c 109 wait(.001); //run outer control loop at 1kHz
elijahsj 4:7a1b35f081bb 110 }
pwensing 0:43448bf056e8 111 // Cleanup after experiment
sangbae 17:98e298577f09 112 ControlLoop.detach();
pwensing 0:43448bf056e8 113 server.setExperimentComplete();
elijahsj 12:84a6dcb60422 114 motorShield.motorAWrite(0, 0); //turn motor A off
pwensing 0:43448bf056e8 115 } // end if
pwensing 0:43448bf056e8 116 } // end while
elijahsj 10:a40d180c305c 117
elijahsj 6:1faceb53dabe 118 } // end main
elijahsj 6:1faceb53dabe 119