nucho
/
RTno_MotorControl
Diff: main.cpp
- Revision:
- 0:3c49891bc39d
- Child:
- 1:7f0fc0d1f777
diff -r 000000000000 -r 3c49891bc39d main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jul 29 11:23:44 2011 +0000 @@ -0,0 +1,188 @@ +#include "mbed.h" +#include "RTno.h" + +#include "QEI.h" +#include "SimplePID.h" +#define MOTOR_OFFSET 1460 +#define KP 5.1 +#define KI 0.0 +#define KD 0.0 +#define RATE 0.2 + +PwmOut motor1(p21); +QEI qei_motor1(p29, p30, NC, 624); +SimplePID pid_motor1(KP,KI,KD,RATE); + +PwmOut motor2(p22); +QEI qei_motor2(p27, p28, NC, 624); +SimplePID pid_motor2(KP,KI,KD,RATE); +/** + * digitalInOut.pde + * RTno is RT-middleware and arduino. + * + * Using RTno, arduino device can communicate any RT-components + * through the RTno-proxy component which is launched in PC. + * Connect arduino with USB, and program with RTno library. + * You do not have to define any protocols to establish communication + * between arduino and PC. + * + * Using RTno, you must not define the function "setup" and "loop". + * Those functions are automatically defined in the RTno libarary. + * You, developers, must define following functions: + * int onInitialize(void); + * int onActivated(void); + * int onDeactivated(void); + * int onExecute(void); + * int onError(void); + * int onReset(void); + * These functions are spontaneously called by the RTno-proxy + * RT-component which is launched in the PC. + */ + + +/** + * This function is called at first. + * conf._default.baudrate: baudrate of serial communication + * exec_cxt.periodic.type: reserved but not used. + */ +void rtcconf(void) { + conf._default.baudrate = 115200; + exec_cxt.periodic.type = ProxySynchronousExecutionContext; +} + +/** + * Declaration Division: + * + * DataPort and Data Buffer should be placed here. + * + * Currently, following 6 types are available. + * TimedLong: + * TimedDouble: + * TimedFloat: + * TimedLongSeq: + * TimedDoubleSeq: + * TimedFloatSeq: + * + * Please refer following comments. If you need to use some ports, + * uncomment the line you want to declare. + **/ +TimedLongSeq position; +InPort positionIn("position", position); + +TimedLongSeq encorder; +OutPort encorderOut("encorder", encorder); + + +////////////////////////////////////////// +// on_initialize +// +// This function is called in the initialization +// sequence. The sequence is triggered by the +// PC. When the RTnoRTC is launched in the PC, +// then, this function is remotely called +// through the USB cable. +// In on_initialize, usually DataPorts are added. +// +////////////////////////////////////////// +int RTno::onInitialize() { + /* Data Ports are added in this section. + */ + addInPort(positionIn); + addOutPort(encorderOut); + + // Some initialization (like port direction setting) + + return RTC_OK; +} + +//////////////////////////////////////////// +// on_activated +// This function is called when the RTnoRTC +// is activated. When the activation, the RTnoRTC +// sends message to call this function remotely. +// If this function is failed (return value +// is RTC_ERROR), RTno will enter ERROR condition. +//////////////////////////////////////////// +int RTno::onActivated() { + // Write here initialization code. + + return RTC_OK; +} + +///////////////////////////////////////////// +// on_deactivated +// This function is called when the RTnoRTC +// is deactivated. +///////////////////////////////////////////// +int RTno::onDeactivated() { + // Write here finalization code. + + return RTC_OK; +} + +////////////////////////////////////////////// +// This function is repeatedly called when the +// RTno is in the ACTIVE condition. +// If this function is failed (return value is +// RTC_ERROR), RTno immediately enter into the +// ERROR condition.r +////////////////////////////////////////////// +int RTno::onExecute() { + + /* + * Input + */ + if (positionIn.isNew()) { + positionIn.read(); + pid_motor1.setGoal(position.data[0]); + pid_motor2.setGoal(position.data[1]); + } + + pid_motor1.setLimits(-500,500); + pid_motor2.setLimits(-500,500); + + /* + * Output + */ + int current1,current2; + current1 = qei_motor1.getPulses(); + current2 = qei_motor2.getPulses(); + + encorder.data.length(2); + encorder.data[0] = current1; + encorder.data[1] = current2; + encorderOut.write(); + + int ctrl1,ctrl2; + ctrl1 = pid_motor1.compute(current1); + ctrl2 = pid_motor2.compute(current2); + + motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET); + motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET); + + return RTC_OK; +} + + +////////////////////////////////////// +// on_error +// This function is repeatedly called when +// the RTno is in the ERROR condition. +// The ERROR condition can be recovered, +// when the RTno is reset. +/////////////////////////////////////// +int RTno::onError() { + return RTC_OK; +} + +//////////////////////////////////////// +// This function is called when +// the RTno is reset. If on_reset is +// succeeded, the RTno will enter into +// the INACTIVE condition. If failed +// (return value is RTC_ERROR), RTno +// will stay in ERROR condition.ec +/////////////////////////////////////// +int RTno::onReset() { + return RTC_OK; +} \ No newline at end of file