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;
}