main.cpp

00001 #include "mbed.h"
00002 #include "RTno.h"
00003 
00004 #include "QEI.h"
00005 #include "SimplePID.h"
00006 #define MOTOR_OFFSET 1460
00007 #define KP 5.1
00008 #define KI 0.0
00009 #define KD 0.0
00010 #define RATE 0.2
00011 
00012 PwmOut motor1(p21);
00013 QEI qei_motor1(p29, p30, NC, 624);
00014 SimplePID pid_motor1(KP,KI,KD,RATE);
00015 
00016 PwmOut motor2(p22);
00017 QEI qei_motor2(p27, p28, NC, 624);
00018 SimplePID pid_motor2(KP,KI,KD,RATE);
00019 /**
00020  * digitalInOut.pde
00021  * RTno is RT-middleware and arduino.
00022  *
00023  * Using RTno, arduino device can communicate any RT-components
00024  *  through the RTno-proxy component which is launched in PC.
00025  * Connect arduino with USB, and program with RTno library.
00026  * You do not have to define any protocols to establish communication
00027  *  between arduino and PC.
00028  *
00029  * Using RTno, you must not define the function "setup" and "loop".
00030  * Those functions are automatically defined in the RTno libarary.
00031  * You, developers, must define following functions:
00032  *  int onInitialize(void);
00033  *  int onActivated(void);
00034  *  int onDeactivated(void);
00035  *  int onExecute(void);
00036  *  int onError(void);
00037  *  int onReset(void);
00038  * These functions are spontaneously called by the RTno-proxy
00039  *  RT-component which is launched in the PC.
00040  */
00041 
00042 
00043 /**
00044  * This function is called at first.
00045  * conf._default.baudrate: baudrate of serial communication
00046  * exec_cxt.periodic.type: reserved but not used.
00047  */
00048 void rtcconf(void) {
00049     conf._default.baudrate = 115200;
00050     exec_cxt.periodic.type = ProxySynchronousExecutionContext;
00051 }
00052 
00053 /**
00054  * Declaration Division:
00055  *
00056  * DataPort and Data Buffer should be placed here.
00057  *
00058  * Currently, following 6 types are available.
00059  * TimedLong:
00060  * TimedDouble:
00061  * TimedFloat:
00062  * TimedLongSeq:
00063  * TimedDoubleSeq:
00064  * TimedFloatSeq:
00065  *
00066  * Please refer following comments. If you need to use some ports,
00067  * uncomment the line you want to declare.
00068  **/
00069 TimedLongSeq position;
00070 InPort positionIn("position", position);
00071 
00072 TimedLongSeq encorder;
00073 OutPort encorderOut("encorder", encorder);
00074 
00075 
00076 
00077 //////////////////////////////////////////
00078 // on_initialize
00079 //
00080 // This function is called in the initialization
00081 // sequence. The sequence is triggered by the
00082 // PC. When the RTnoRTC is launched in the PC,
00083 // then, this function is remotely called
00084 // through the USB cable.
00085 // In on_initialize, usually DataPorts are added.
00086 //
00087 //////////////////////////////////////////
00088 int RTno::onInitialize() {
00089     /* Data Ports are added in this section.
00090     */
00091     addInPort(positionIn);
00092     addOutPort(encorderOut);
00093 
00094     // Some initialization (like port direction setting)
00095     
00096     return RTC_OK;
00097 }
00098 
00099 ////////////////////////////////////////////
00100 // on_activated
00101 // This function is called when the RTnoRTC
00102 // is activated. When the activation, the RTnoRTC
00103 // sends message to call this function remotely.
00104 // If this function is failed (return value
00105 // is RTC_ERROR), RTno will enter ERROR condition.
00106 ////////////////////////////////////////////
00107 int RTno::onActivated() {
00108     // Write here initialization code.
00109 
00110     return RTC_OK;
00111 }
00112 
00113 /////////////////////////////////////////////
00114 // on_deactivated
00115 // This function is called when the RTnoRTC
00116 // is deactivated.
00117 /////////////////////////////////////////////
00118 int RTno::onDeactivated() {
00119     // Write here finalization code.
00120 
00121     return RTC_OK;
00122 }
00123 
00124 //////////////////////////////////////////////
00125 // This function is repeatedly called when the
00126 // RTno is in the ACTIVE condition.
00127 // If this function is failed (return value is
00128 // RTC_ERROR), RTno immediately enter into the
00129 // ERROR condition.r
00130 //////////////////////////////////////////////
00131 int RTno::onExecute() {
00132 
00133     /*
00134      * Input 
00135      */
00136     if (positionIn.isNew()) {
00137         positionIn.read();
00138         pid_motor1.setGoal(position.data[0]);
00139         pid_motor2.setGoal(position.data[1]);
00140     }
00141     
00142     pid_motor1.setLimits(-500,500);
00143     pid_motor2.setLimits(-500,500);
00144 
00145     /*
00146      * Output 
00147      */
00148     int current1,current2;
00149     current1 = qei_motor1.getPulses();
00150     current2 = qei_motor2.getPulses();
00151 
00152     encorder.data.length(2);    
00153     encorder.data[0] = current1;
00154     encorder.data[1] = current2;
00155     encorderOut.write();
00156 
00157     int ctrl1,ctrl2;
00158     ctrl1 = pid_motor1.compute(current1);
00159     ctrl2 = pid_motor2.compute(current2);
00160     
00161     motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET);
00162     motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET);
00163 
00164     return RTC_OK;
00165 }
00166 
00167 
00168 //////////////////////////////////////
00169 // on_error
00170 // This function is repeatedly called when
00171 // the RTno is in the ERROR condition.
00172 // The ERROR condition can be recovered,
00173 // when the RTno is reset.
00174 ///////////////////////////////////////
00175 int RTno::onError() {
00176     return RTC_OK;
00177 }
00178 
00179 ////////////////////////////////////////
00180 // This function is called when
00181 // the RTno is reset. If on_reset is
00182 // succeeded, the RTno will enter into
00183 // the INACTIVE condition. If failed
00184 // (return value is RTC_ERROR), RTno
00185 // will stay in ERROR condition.ec
00186 ///////////////////////////////////////
00187 int RTno::onReset() {
00188     return RTC_OK;
00189 }