nucho
/
RTno_MotorControl
main.cpp@0:3c49891bc39d, 2011-07-29 (annotated)
- Committer:
- nucho
- Date:
- Fri Jul 29 11:23:44 2011 +0000
- Revision:
- 0:3c49891bc39d
- Child:
- 1:7f0fc0d1f777
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 0:3c49891bc39d | 1 | #include "mbed.h" |
nucho | 0:3c49891bc39d | 2 | #include "RTno.h" |
nucho | 0:3c49891bc39d | 3 | |
nucho | 0:3c49891bc39d | 4 | #include "QEI.h" |
nucho | 0:3c49891bc39d | 5 | #include "SimplePID.h" |
nucho | 0:3c49891bc39d | 6 | #define MOTOR_OFFSET 1460 |
nucho | 0:3c49891bc39d | 7 | #define KP 5.1 |
nucho | 0:3c49891bc39d | 8 | #define KI 0.0 |
nucho | 0:3c49891bc39d | 9 | #define KD 0.0 |
nucho | 0:3c49891bc39d | 10 | #define RATE 0.2 |
nucho | 0:3c49891bc39d | 11 | |
nucho | 0:3c49891bc39d | 12 | PwmOut motor1(p21); |
nucho | 0:3c49891bc39d | 13 | QEI qei_motor1(p29, p30, NC, 624); |
nucho | 0:3c49891bc39d | 14 | SimplePID pid_motor1(KP,KI,KD,RATE); |
nucho | 0:3c49891bc39d | 15 | |
nucho | 0:3c49891bc39d | 16 | PwmOut motor2(p22); |
nucho | 0:3c49891bc39d | 17 | QEI qei_motor2(p27, p28, NC, 624); |
nucho | 0:3c49891bc39d | 18 | SimplePID pid_motor2(KP,KI,KD,RATE); |
nucho | 0:3c49891bc39d | 19 | /** |
nucho | 0:3c49891bc39d | 20 | * digitalInOut.pde |
nucho | 0:3c49891bc39d | 21 | * RTno is RT-middleware and arduino. |
nucho | 0:3c49891bc39d | 22 | * |
nucho | 0:3c49891bc39d | 23 | * Using RTno, arduino device can communicate any RT-components |
nucho | 0:3c49891bc39d | 24 | * through the RTno-proxy component which is launched in PC. |
nucho | 0:3c49891bc39d | 25 | * Connect arduino with USB, and program with RTno library. |
nucho | 0:3c49891bc39d | 26 | * You do not have to define any protocols to establish communication |
nucho | 0:3c49891bc39d | 27 | * between arduino and PC. |
nucho | 0:3c49891bc39d | 28 | * |
nucho | 0:3c49891bc39d | 29 | * Using RTno, you must not define the function "setup" and "loop". |
nucho | 0:3c49891bc39d | 30 | * Those functions are automatically defined in the RTno libarary. |
nucho | 0:3c49891bc39d | 31 | * You, developers, must define following functions: |
nucho | 0:3c49891bc39d | 32 | * int onInitialize(void); |
nucho | 0:3c49891bc39d | 33 | * int onActivated(void); |
nucho | 0:3c49891bc39d | 34 | * int onDeactivated(void); |
nucho | 0:3c49891bc39d | 35 | * int onExecute(void); |
nucho | 0:3c49891bc39d | 36 | * int onError(void); |
nucho | 0:3c49891bc39d | 37 | * int onReset(void); |
nucho | 0:3c49891bc39d | 38 | * These functions are spontaneously called by the RTno-proxy |
nucho | 0:3c49891bc39d | 39 | * RT-component which is launched in the PC. |
nucho | 0:3c49891bc39d | 40 | */ |
nucho | 0:3c49891bc39d | 41 | |
nucho | 0:3c49891bc39d | 42 | |
nucho | 0:3c49891bc39d | 43 | /** |
nucho | 0:3c49891bc39d | 44 | * This function is called at first. |
nucho | 0:3c49891bc39d | 45 | * conf._default.baudrate: baudrate of serial communication |
nucho | 0:3c49891bc39d | 46 | * exec_cxt.periodic.type: reserved but not used. |
nucho | 0:3c49891bc39d | 47 | */ |
nucho | 0:3c49891bc39d | 48 | void rtcconf(void) { |
nucho | 0:3c49891bc39d | 49 | conf._default.baudrate = 115200; |
nucho | 0:3c49891bc39d | 50 | exec_cxt.periodic.type = ProxySynchronousExecutionContext; |
nucho | 0:3c49891bc39d | 51 | } |
nucho | 0:3c49891bc39d | 52 | |
nucho | 0:3c49891bc39d | 53 | /** |
nucho | 0:3c49891bc39d | 54 | * Declaration Division: |
nucho | 0:3c49891bc39d | 55 | * |
nucho | 0:3c49891bc39d | 56 | * DataPort and Data Buffer should be placed here. |
nucho | 0:3c49891bc39d | 57 | * |
nucho | 0:3c49891bc39d | 58 | * Currently, following 6 types are available. |
nucho | 0:3c49891bc39d | 59 | * TimedLong: |
nucho | 0:3c49891bc39d | 60 | * TimedDouble: |
nucho | 0:3c49891bc39d | 61 | * TimedFloat: |
nucho | 0:3c49891bc39d | 62 | * TimedLongSeq: |
nucho | 0:3c49891bc39d | 63 | * TimedDoubleSeq: |
nucho | 0:3c49891bc39d | 64 | * TimedFloatSeq: |
nucho | 0:3c49891bc39d | 65 | * |
nucho | 0:3c49891bc39d | 66 | * Please refer following comments. If you need to use some ports, |
nucho | 0:3c49891bc39d | 67 | * uncomment the line you want to declare. |
nucho | 0:3c49891bc39d | 68 | **/ |
nucho | 0:3c49891bc39d | 69 | TimedLongSeq position; |
nucho | 0:3c49891bc39d | 70 | InPort positionIn("position", position); |
nucho | 0:3c49891bc39d | 71 | |
nucho | 0:3c49891bc39d | 72 | TimedLongSeq encorder; |
nucho | 0:3c49891bc39d | 73 | OutPort encorderOut("encorder", encorder); |
nucho | 0:3c49891bc39d | 74 | |
nucho | 0:3c49891bc39d | 75 | |
nucho | 0:3c49891bc39d | 76 | ////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 77 | // on_initialize |
nucho | 0:3c49891bc39d | 78 | // |
nucho | 0:3c49891bc39d | 79 | // This function is called in the initialization |
nucho | 0:3c49891bc39d | 80 | // sequence. The sequence is triggered by the |
nucho | 0:3c49891bc39d | 81 | // PC. When the RTnoRTC is launched in the PC, |
nucho | 0:3c49891bc39d | 82 | // then, this function is remotely called |
nucho | 0:3c49891bc39d | 83 | // through the USB cable. |
nucho | 0:3c49891bc39d | 84 | // In on_initialize, usually DataPorts are added. |
nucho | 0:3c49891bc39d | 85 | // |
nucho | 0:3c49891bc39d | 86 | ////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 87 | int RTno::onInitialize() { |
nucho | 0:3c49891bc39d | 88 | /* Data Ports are added in this section. |
nucho | 0:3c49891bc39d | 89 | */ |
nucho | 0:3c49891bc39d | 90 | addInPort(positionIn); |
nucho | 0:3c49891bc39d | 91 | addOutPort(encorderOut); |
nucho | 0:3c49891bc39d | 92 | |
nucho | 0:3c49891bc39d | 93 | // Some initialization (like port direction setting) |
nucho | 0:3c49891bc39d | 94 | |
nucho | 0:3c49891bc39d | 95 | return RTC_OK; |
nucho | 0:3c49891bc39d | 96 | } |
nucho | 0:3c49891bc39d | 97 | |
nucho | 0:3c49891bc39d | 98 | //////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 99 | // on_activated |
nucho | 0:3c49891bc39d | 100 | // This function is called when the RTnoRTC |
nucho | 0:3c49891bc39d | 101 | // is activated. When the activation, the RTnoRTC |
nucho | 0:3c49891bc39d | 102 | // sends message to call this function remotely. |
nucho | 0:3c49891bc39d | 103 | // If this function is failed (return value |
nucho | 0:3c49891bc39d | 104 | // is RTC_ERROR), RTno will enter ERROR condition. |
nucho | 0:3c49891bc39d | 105 | //////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 106 | int RTno::onActivated() { |
nucho | 0:3c49891bc39d | 107 | // Write here initialization code. |
nucho | 0:3c49891bc39d | 108 | |
nucho | 0:3c49891bc39d | 109 | return RTC_OK; |
nucho | 0:3c49891bc39d | 110 | } |
nucho | 0:3c49891bc39d | 111 | |
nucho | 0:3c49891bc39d | 112 | ///////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 113 | // on_deactivated |
nucho | 0:3c49891bc39d | 114 | // This function is called when the RTnoRTC |
nucho | 0:3c49891bc39d | 115 | // is deactivated. |
nucho | 0:3c49891bc39d | 116 | ///////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 117 | int RTno::onDeactivated() { |
nucho | 0:3c49891bc39d | 118 | // Write here finalization code. |
nucho | 0:3c49891bc39d | 119 | |
nucho | 0:3c49891bc39d | 120 | return RTC_OK; |
nucho | 0:3c49891bc39d | 121 | } |
nucho | 0:3c49891bc39d | 122 | |
nucho | 0:3c49891bc39d | 123 | ////////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 124 | // This function is repeatedly called when the |
nucho | 0:3c49891bc39d | 125 | // RTno is in the ACTIVE condition. |
nucho | 0:3c49891bc39d | 126 | // If this function is failed (return value is |
nucho | 0:3c49891bc39d | 127 | // RTC_ERROR), RTno immediately enter into the |
nucho | 0:3c49891bc39d | 128 | // ERROR condition.r |
nucho | 0:3c49891bc39d | 129 | ////////////////////////////////////////////// |
nucho | 0:3c49891bc39d | 130 | int RTno::onExecute() { |
nucho | 0:3c49891bc39d | 131 | |
nucho | 0:3c49891bc39d | 132 | /* |
nucho | 0:3c49891bc39d | 133 | * Input |
nucho | 0:3c49891bc39d | 134 | */ |
nucho | 0:3c49891bc39d | 135 | if (positionIn.isNew()) { |
nucho | 0:3c49891bc39d | 136 | positionIn.read(); |
nucho | 0:3c49891bc39d | 137 | pid_motor1.setGoal(position.data[0]); |
nucho | 0:3c49891bc39d | 138 | pid_motor2.setGoal(position.data[1]); |
nucho | 0:3c49891bc39d | 139 | } |
nucho | 0:3c49891bc39d | 140 | |
nucho | 0:3c49891bc39d | 141 | pid_motor1.setLimits(-500,500); |
nucho | 0:3c49891bc39d | 142 | pid_motor2.setLimits(-500,500); |
nucho | 0:3c49891bc39d | 143 | |
nucho | 0:3c49891bc39d | 144 | /* |
nucho | 0:3c49891bc39d | 145 | * Output |
nucho | 0:3c49891bc39d | 146 | */ |
nucho | 0:3c49891bc39d | 147 | int current1,current2; |
nucho | 0:3c49891bc39d | 148 | current1 = qei_motor1.getPulses(); |
nucho | 0:3c49891bc39d | 149 | current2 = qei_motor2.getPulses(); |
nucho | 0:3c49891bc39d | 150 | |
nucho | 0:3c49891bc39d | 151 | encorder.data.length(2); |
nucho | 0:3c49891bc39d | 152 | encorder.data[0] = current1; |
nucho | 0:3c49891bc39d | 153 | encorder.data[1] = current2; |
nucho | 0:3c49891bc39d | 154 | encorderOut.write(); |
nucho | 0:3c49891bc39d | 155 | |
nucho | 0:3c49891bc39d | 156 | int ctrl1,ctrl2; |
nucho | 0:3c49891bc39d | 157 | ctrl1 = pid_motor1.compute(current1); |
nucho | 0:3c49891bc39d | 158 | ctrl2 = pid_motor2.compute(current2); |
nucho | 0:3c49891bc39d | 159 | |
nucho | 0:3c49891bc39d | 160 | motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET); |
nucho | 0:3c49891bc39d | 161 | motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET); |
nucho | 0:3c49891bc39d | 162 | |
nucho | 0:3c49891bc39d | 163 | return RTC_OK; |
nucho | 0:3c49891bc39d | 164 | } |
nucho | 0:3c49891bc39d | 165 | |
nucho | 0:3c49891bc39d | 166 | |
nucho | 0:3c49891bc39d | 167 | ////////////////////////////////////// |
nucho | 0:3c49891bc39d | 168 | // on_error |
nucho | 0:3c49891bc39d | 169 | // This function is repeatedly called when |
nucho | 0:3c49891bc39d | 170 | // the RTno is in the ERROR condition. |
nucho | 0:3c49891bc39d | 171 | // The ERROR condition can be recovered, |
nucho | 0:3c49891bc39d | 172 | // when the RTno is reset. |
nucho | 0:3c49891bc39d | 173 | /////////////////////////////////////// |
nucho | 0:3c49891bc39d | 174 | int RTno::onError() { |
nucho | 0:3c49891bc39d | 175 | return RTC_OK; |
nucho | 0:3c49891bc39d | 176 | } |
nucho | 0:3c49891bc39d | 177 | |
nucho | 0:3c49891bc39d | 178 | //////////////////////////////////////// |
nucho | 0:3c49891bc39d | 179 | // This function is called when |
nucho | 0:3c49891bc39d | 180 | // the RTno is reset. If on_reset is |
nucho | 0:3c49891bc39d | 181 | // succeeded, the RTno will enter into |
nucho | 0:3c49891bc39d | 182 | // the INACTIVE condition. If failed |
nucho | 0:3c49891bc39d | 183 | // (return value is RTC_ERROR), RTno |
nucho | 0:3c49891bc39d | 184 | // will stay in ERROR condition.ec |
nucho | 0:3c49891bc39d | 185 | /////////////////////////////////////// |
nucho | 0:3c49891bc39d | 186 | int RTno::onReset() { |
nucho | 0:3c49891bc39d | 187 | return RTC_OK; |
nucho | 0:3c49891bc39d | 188 | } |