Diff: main.cpp

Revision:

0:3c49891bc39d

Child:

1:7f0fc0d1f777

--- /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