File content as of revision 1:7f0fc0d1f777:
#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;
}