Skip to content

Commit

Permalink
add pendulum example, fix missing MEL_API in serial port
Browse files Browse the repository at this point in the history
  • Loading branch information
epezent committed Feb 3, 2019
1 parent 25942c4 commit 635c987
Show file tree
Hide file tree
Showing 4 changed files with 98 additions and 1 deletion.
1 change: 1 addition & 0 deletions examples/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ if(QUANSER)
mel_example(haptic_paddle)
mel_example(overview)
mel_example(ati_sensor)
mel_example(pendulum)
endif()

if(NI_ARM)
Expand Down
95 changes: 95 additions & 0 deletions examples/ex_pendulum.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
// MIT License
//
// MEL - Mechatronics Engine & Library
// Copyright (c) 2019 Mechatronics and Haptic Interfaces Lab - Rice University
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// Author(s): Evan Pezent (epezent@rice.edu)

#include <MEL/Daq/Quanser/Q8Usb.hpp>
#include <MEL/Mechatronics.hpp>
#include <MEL/Communications.hpp>
#include <MEL/Core.hpp>
#include <MEL/Math.hpp>
#include <MEL/Devices/Windows/Keyboard.hpp>

using namespace mel;

// create global stop variable CTRL-C handler function
ctrl_bool stop(false);
bool handler(CtrlEvent event) {
if (event == CtrlEvent::CtrlC)
stop = true;
return true;
}

int main() {
// register handler to gracefully exit program
register_ctrl_handler(handler);
// create Q8 USB DAQ
Q8Usb q8;
q8.open();
q8.enable();
// set encoder deg/count
q8.encoder[0].set_units_per_count(2 * PI / 500);
q8.encoder[0].zero();
// melshare for external scoping
MelShare ms("pendulum");
// constants
const double sense_gain = 2.20; // A/V
const double commd_gain = 0.5; // A/V
const double kp = 5.0; // A/rad
const double kd = 0.5; // A*s/rad
const double ref = PI; // radians
// PD controlelr (I = 0)
PidController pd(kp, kd, 0);
// control loop timer
Timer timer(hertz(1000));
// start Q8 watchdog
q8.watchdog.start();
while (!stop) {
// get current time
Time t = timer.get_elapsed_time_actual();
// sync Q8 input with real world
q8.update_input();
// enable/disable pendulum
if (Keyboard::is_key_pressed(Key::Up)) // enable
q8.DO[0].set_value(High);
else if (Keyboard::is_key_pressed(Key::Down)) // disable
q8.DO[0].set_value(Low);
// get current pendulum state
double pos = q8.encoder[0].get_position(); // rad
double vel = q8.encoder[0].get_velocity(); // rad/s
// wrap position to pi so controller behaves
pos = wrap_to_pi(pos); // rad
// measure actual current
double sense_volt = q8.AI[0].get_value(); // V
double sense_curr = sense_volt * sense_gain; // A
// compute PD controller command
double commd_curr = pd.calculate(ref, pos, vel, t); // A
double commd_volt = commd_curr / commd_gain; // V
// set command
q8.AO[0].set_value(commd_volt);
// write data to melshare for scoping
ms.write_data({ pos, vel, sense_curr, commd_curr });
// sync output w/ real world
q8.update_output();
// kick watchdog
q8.watchdog.kick();
// wait for next timestep
timer.wait();
}
// shutdown Q8
q8.disable();
q8.close();
return 0;
}
2 changes: 1 addition & 1 deletion include/MEL/Communications/SerialPort.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
namespace mel {

/// Interface to RS-232 Serial Port
class SerialPort : NonCopyable {
class MEL_API SerialPort : NonCopyable {
public:

/// Constructor
Expand Down
1 change: 1 addition & 0 deletions include/MEL/Mechatronics.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <MEL/Mechatronics/Limiter.hpp>
#include <MEL/Mechatronics/Motor.hpp>
#include <MEL/Mechatronics/PdController.hpp>
#include <MEL/Mechatronics/PidController.hpp>
#include <MEL/Mechatronics/PositionSensor.hpp>
#include <MEL/Mechatronics/Robot.hpp>
#include <MEL/Mechatronics/VelocitySensor.hpp>
Expand Down

0 comments on commit 635c987

Please sign in to comment.