Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#include "DummyServer.h"
void DummyServer::event(float * buffer){
for(int i = 0; i < myPumberOfPidChannels; i++)
{
buffer[(i*3)+0] = 0;
buffer[(i*3)+1] = 0;
buffer[(i*3)+2] = 0;
}
// DO something
// set values back to Buffer
}
/**
* RBE3001 - Nucleo Firmware
*
* Instructions
* ------------
* Welcome! This is the main file of the Nucleo C++ firmware.
* The code in this source file starts all the control and communication loops
* required to control the arm. Please, take some time to familiarize yourself with the
* workflow of the program.
*
* IMPORTANT - this program has two distinct running modes:
* **Dummy mode** to be used for firmware testing/debugging when no arm is connected.
* **Physical mode** to be used when the robotic arm is connected to the Nucleo board.
*
* The running mode can be selected at compile time by commenting/uncommenting the
* DUMMYMODE macro below.
*
*/
#include "main.h"
#define DOFs 3 // this macro defines the number of joints of the robotic arm
//#define DUMMYMODE // this macro selects the running mode - see instructions above
#define DEBUG_ // if defined, this macro enables the printing of debug
// statements to the serial port - which can be read with PUTTY
/*
* ======= PART 1: Global Variables and definition of ancillary functions ======
*/
Ticker pidTimer; // implements a timer
static PIDimp * pid[DOFs]; // pointer to PID controllers (one for each link)
HIDSimplePacket coms; // HID packet handlers
// The following array contains the "home" positions (in encoder ticks) for each
// of the robot's joints
float homePosition[3] = {0,0,0};
void runPid() {
// update all positions fast and together
for (int i = 0; i < DOFs; i++)
pid[i]->updatePosition();
// next update all control outputs
for (int i = 0; i < DOFs; i++)
pid[i]->updateControl();
}
/*
* ======= PART 2: Main file ===================================================
*/
int main() {
#ifdef DEBUG
printf("\r\n\r\n RBE3001 Arm Firmware \r\n\r\n");
printf("\r\n\r\n Initializing... \r\n\r\n");
#endif
/*
* ======= PART 2a: Initialize PID control ===================================
* In this section we instantiate objects that implement PID control for
* each of the joints.
*/
#ifdef DUMMYMODE
for (int i = 0; i < 3; i++)
pid[i] = (PIDimp*) new DummyPID();
#else
#if defined( REV1)
SPI * spiDev = new SPI(MOSI, MISO, CLK);
SPI * spi3 = spiDev;
SPI * spi4 = spiDev;
SPI * spi5 = spiDev;
#else if defined(REV2)
SPI * spi3 = new SPI(PC_12, PC_11, PC_10); // spi(mosi, miso, clk)
SPI * spi4 = new SPI(PE_6, PE_5, PE_2); // spi(mosi, miso, clk)
SPI * spi5 = new SPI(PF_9, PF_8, PF_7); // spi(mosi, miso, clk)
#endif
pid[0] = new PIDimp(new Servo(SERVO_1, 5), new AS5050(spi3, ENC_1),
new AnalogIn(LOAD_1)); // mosi, miso, sclk, cs
pid[1] = new PIDimp(new Servo(SERVO_2, 5), new AS5050(spi4, ENC_2),
new AnalogIn(LOAD_2)); // mosi, miso, sclk, cs
pid[2] = new PIDimp(new Servo(SERVO_3, 5), new AS5050(spi5, ENC_3),
new AnalogIn(LOAD_3)); // mosi, miso, sclk, cs
#endif
RunEveryObject * print = new RunEveryObject(0, 100);
// disable PID
for (int i = 0; i < DOFs; i++)
pid[i]->state.config.Enabled = false;
// Cosines delay
wait_ms(500);
pidTimer.attach(&runPid, 0.0025);
// capture 100 ms of encoders before starting
wait_ms(100);
for (int i = 0; i < DOFs; i++) // for each joint,
{
// reset the PID control after encoders have been updated a few times
pid[i]->InitilizePidController();
// we will now "zero" the encoder readings
#ifdef DUMMYMODE // if operating in Dummy Mode, set the initial encoder reading to zero
pid[i]->ZeroPID();
#else // else, use the values in homePosition
pid[i]->pidReset(pid[i]->GetPIDPosition() - homePosition[i]);
#endif
// !FIXME Do we need the following two instructions? I'm afraid this may generate
// strange behaviors.
if (pid[i]->GetPIDPosition() > 3000)
pid[i]->pidReset(pid[i]->GetPIDPosition() - 4095);
// Finally, enable PID control
pid[i]->SetPIDEnabled(true);
pid[i]->SetPIDTimed(pid[i]->GetPIDPosition(), 1000); // !FIXME What does this instruction do?
}
/*
* ======= PART 2b: Initialize HID communication =============================
* In this section we instatiate objects that handle the communication between
* this firmware and MATLAB. Each of thess objects implements a server that responds
* to commands sent over HID. During RBE3001, you will be asked to
* implement your own communication servers. To instantiate a new server, use the
* template below:
*
* coms.attach(new 'MyServerName'(pid, DOFs));
*
* C++ classes that define servers should be placed under /src/coms
*
* IMPORTANT: when adding a new server, do not forget to add its definition
* by including the relevant header file at the beginning of this
* source file
*/
coms.attach(new PidServer(pid, DOFs));
//coms.attach(new PidConfigServer(pid, DOFs));
#ifdef DEBUG_
printf("\r\n\r\n Initialization complete. \r\n\r\n");
printf("\r\n\r\n Starting main loop... \r\n\r\n");
#endif
/*
* ======= PART 2c: Main loop ================================================
* We are now ready to run the main loop of the firmware. The most important
* instruction within the main loop is `coms.server()'. This method, which is
* invoked at each loop iteration, results in the execution of the `event()'
* method on each communication object that was instantiated above
* (see Part 2b of source file). The `event()' method implements upstream/downstream
* communication between this firmware and MATLAB - for more information on this
* topic, see the example provided in /src/coms/PidServer.cpp.
*
* IMPORTANT: the code below includes print statements that, while being useful for
* debugging/testing, will slow down the main loop execution rate.
* You can disable these statements at compile time, by commenting out
* the DEBUG macro at the beginning of this source file.
*/
while (1) {
//I'll use PA_7, to write it using input from PA_6 each time through the loop
DigitalOut led(PA_7);
DigitalIn button(PA_6);
led.write(button);
coms.server();
// The following code prints out debug statements.
#ifdef DEBUG_
// print encoder values for each joint
printf("\r\nEncoder Value = %f , %f , %f", pid[0]->GetPIDPosition(),
pid[1]->GetPIDPosition(), pid[2]->GetPIDPosition());
// print load cell readings
printf(" Setpoint = %f , %f , %f", pid[0]->state.SetPoint,
pid[1]->state.SetPoint, pid[2]->state.SetPoint);
printf(" Gravity = %f , %f , %f", pid[0]->gravityCompTerm,
pid[1]->gravityCompTerm, pid[2]->gravityCompTerm);
printf(" Load Value = %f , %f , %f", pid[0]->loadCell->read(),
pid[1]->loadCell->read(), pid[2]->loadCell->read());
#endif // DEBUG
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.