Akito / Mbed 2 deprecated kisoken_PenPlotter

４年工学基礎研究（前期）で作成したペンプロッタの制御プログラム

Dependencies:   SPM_simple mbed trapezoidalMotionCal

main.cpp@3:ddb5ca27e94b, 2017-08-04 (annotated)

Committer:
Akito914
Date:
Fri Aug 04 03:47:41 2017 +0000
Revision:
3:ddb5ca27e94b
Parent:
2:80eb19e9091e 
?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Akito914 0:0eddebdc2c58 1 #include "mbed.h"
Akito914 0:0eddebdc2c58 2 #include "SPM.h"
Akito914 0:0eddebdc2c58 3 #include "trapezoidalMotionCal.h"
Akito914 0:0eddebdc2c58 4
Akito914 1:3663d0f20136 5 #define PEN_SAVEMODE 120
Akito914 1:3663d0f20136 6 #define PEN_UP 90
Akito914 1:3663d0f20136 7 #define PEN_DOWN 70
Akito914 1:3663d0f20136 8
Akito914 1:3663d0f20136 9
Akito914 2:80eb19e9091e 10 //stepMotor x_axis(PC_4 ,PB_13 ,PB_14 ,PB_15);
Akito914 0:0eddebdc2c58 11 stepMotor x_axis(PB_15 ,PB_14 ,PB_13 ,PC_4);
Akito914 2:80eb19e9091e 12 //stepMotor y_axis(PB_1 ,PB_2 ,PB_12 ,PA_11);
Akito914 2:80eb19e9091e 13 stepMotor y_axis(PA_11 ,PB_12 ,PB_2 ,PB_1);
Akito914 0:0eddebdc2c58 14
Akito914 3:ddb5ca27e94b 15 trapezoidalMotionCal motionCal(50, 50, 500, 5000, -5000);
Akito914 0:0eddebdc2c58 16
Akito914 0:0eddebdc2c58 17 PwmOut servo(PC_6);
Akito914 1:3663d0f20136 18 PwmOut speaker(/*PC_8*/PA_15);
Akito914 0:0eddebdc2c58 19
Akito914 0:0eddebdc2c58 20 DigitalIn x_originSW(PA_0);
Akito914 0:0eddebdc2c58 21 DigitalIn y_originSW(PA_1);
Akito914 0:0eddebdc2c58 22
Akito914 3:ddb5ca27e94b 23 DigitalOut myled(PA_5);
Akito914 0:0eddebdc2c58 24 Serial pc(USBTX,USBRX);
Akito914 2:80eb19e9091e 25 Serial debugPort(PA_9,PA_10);
Akito914 0:0eddebdc2c58 26
Akito914 0:0eddebdc2c58 27 typedef struct {
Akito914 0:0eddebdc2c58 28 int32_t x;
Akito914 0:0eddebdc2c58 29 int32_t y;
Akito914 0:0eddebdc2c58 30 } coordinate_t;
Akito914 0:0eddebdc2c58 31
Akito914 0:0eddebdc2c58 32 int task = 0;
Akito914 0:0eddebdc2c58 33 int32_t pulse = 0;
Akito914 0:0eddebdc2c58 34
Akito914 1:3663d0f20136 35 int job = 0;
Akito914 1:3663d0f20136 36 int nextMode = 0;
Akito914 1:3663d0f20136 37 int penMotion = 0; // 0: up 1: down
Akito914 1:3663d0f20136 38 coordinate_t nextPosition = {0};
Akito914 0:0eddebdc2c58 39
Akito914 1:3663d0f20136 40 coordinate_t currentPosition = {0};
Akito914 1:3663d0f20136 41
Akito914 1:3663d0f20136 42 void pcIntr();
Akito914 1:3663d0f20136 43 void rcdataProcessing(int32_t *RXData);
Akito914 1:3663d0f20136 44 void sendRequest();
Akito914 1:3663d0f20136 45
Akito914 0:0eddebdc2c58 46 void driveServo(int deg);
Akito914 0:0eddebdc2c58 47 void linearInterpolation(coordinate_t cStart, coordinate_t cTarg);
Akito914 0:0eddebdc2c58 48 void trapezoid(int32_t targSteps);
Akito914 0:0eddebdc2c58 49
Akito914 1:3663d0f20136 50 void beep(int T_us,int t_ms);
Akito914 1:3663d0f20136 51 void pi(int times);
Akito914 1:3663d0f20136 52 void q_sound(void);
Akito914 1:3663d0f20136 53 void ans_sound(void);
Akito914 1:3663d0f20136 54 void piroro(void);
Akito914 0:0eddebdc2c58 55
Akito914 1:3663d0f20136 56 // 受信割込みハンドラ
Akito914 1:3663d0f20136 57 void pcIntr()
Akito914 1:3663d0f20136 58 {
Akito914 1:3663d0f20136 59 static int32_t RXByte[8] = {0};
Akito914 1:3663d0f20136 60 static int32_t RXCount = 0;
Akito914 1:3663d0f20136 61
Akito914 1:3663d0f20136 62 char temp = pc.getc();
Akito914 1:3663d0f20136 63
Akito914 1:3663d0f20136 64 if(((temp >> 5) & 0x07) != RXCount){
Akito914 1:3663d0f20136 65 RXCount = 0;
Akito914 1:3663d0f20136 66 }
Akito914 1:3663d0f20136 67 else{
Akito914 1:3663d0f20136 68 RXByte[RXCount] = temp;
Akito914 1:3663d0f20136 69
Akito914 1:3663d0f20136 70 if(RXCount < 7){
Akito914 1:3663d0f20136 71 RXCount += 1;
Akito914 1:3663d0f20136 72 }
Akito914 1:3663d0f20136 73 else{ // all data received
Akito914 1:3663d0f20136 74 RXCount = 0;
Akito914 1:3663d0f20136 75 rcdataProcessing(RXByte);
Akito914 1:3663d0f20136 76 }
Akito914 1:3663d0f20136 77 }
Akito914 1:3663d0f20136 78 }
Akito914 0:0eddebdc2c58 79
Akito914 1:3663d0f20136 80
Akito914 1:3663d0f20136 81 void rcdataProcessing(int32_t *RXData){
Akito914 1:3663d0f20136 82
Akito914 1:3663d0f20136 83 if((RXData[0] & 0x08) != 0){ // move mode
Akito914 1:3663d0f20136 84 nextPosition.x = (RXData[1] & 0x0f) | ((RXData[2] & 0x0f) << 4) | ((RXData[3] & 0x0f) << 8);
Akito914 1:3663d0f20136 85 nextPosition.y = (RXData[4] & 0x0f) | ((RXData[5] & 0x0f) << 4) | ((RXData[6] & 0x0f) << 8);
Akito914 1:3663d0f20136 86 nextMode = 1;
Akito914 0:0eddebdc2c58 87 }
Akito914 1:3663d0f20136 88 else{ // pen mode
Akito914 1:3663d0f20136 89 nextMode = 0;
Akito914 1:3663d0f20136 90 penMotion = RXData[0] & 0x01;
Akito914 1:3663d0f20136 91 }
Akito914 1:3663d0f20136 92 job = 1;
Akito914 1:3663d0f20136 93 }
Akito914 1:3663d0f20136 94
Akito914 1:3663d0f20136 95
Akito914 1:3663d0f20136 96 void sendRequest(){
Akito914 1:3663d0f20136 97 pc.putc(0x01);
Akito914 0:0eddebdc2c58 98 }
Akito914 0:0eddebdc2c58 99
Akito914 0:0eddebdc2c58 100 void driveServo(int deg){
Akito914 0:0eddebdc2c58 101 const static int servo_offset =830;
Akito914 0:0eddebdc2c58 102 const static float servo_gain =10.0;
Akito914 0:0eddebdc2c58 103
Akito914 0:0eddebdc2c58 104 deg=deg*servo_gain+servo_offset;
Akito914 0:0eddebdc2c58 105 servo.pulsewidth_us(deg);
Akito914 0:0eddebdc2c58 106
Akito914 0:0eddebdc2c58 107 return;
Akito914 0:0eddebdc2c58 108 }
Akito914 0:0eddebdc2c58 109
Akito914 0:0eddebdc2c58 110 int main()
Akito914 0:0eddebdc2c58 111 {
Akito914 2:80eb19e9091e 112 coordinate_t runningPosition = {0};
Akito914 2:80eb19e9091e 113 int runningPenMotion = 0;
Akito914 1:3663d0f20136 114
Akito914 1:3663d0f20136 115 pc.baud(115200);
Akito914 2:80eb19e9091e 116 debugPort.baud(115200);
Akito914 1:3663d0f20136 117
Akito914 1:3663d0f20136 118 pc.attach(pcIntr, Serial::RxIrq);
Akito914 2:80eb19e9091e 119
Akito914 2:80eb19e9091e 120 wait_ms(1000);
Akito914 2:80eb19e9091e 121
Akito914 2:80eb19e9091e 122 debugPort.printf("Hello Debug Port\r\n");
Akito914 2:80eb19e9091e 123
Akito914 0:0eddebdc2c58 124 x_axis.lock();
Akito914 0:0eddebdc2c58 125 y_axis.lock();
Akito914 2:80eb19e9091e 126
Akito914 1:3663d0f20136 127 driveServo(PEN_UP);
Akito914 1:3663d0f20136 128 /*
Akito914 1:3663d0f20136 129 while(1){
Akito914 1:3663d0f20136 130 nextPosition.x = 100; nextPosition.y = 200;
Akito914 1:3663d0f20136 131 linearInterpolation(currentPosition, nextPosition);
Akito914 1:3663d0f20136 132 currentPosition = nextPosition;
Akito914 1:3663d0f20136 133 wait_ms(100);
Akito914 1:3663d0f20136 134 nextPosition.x = 100; nextPosition.y = 0;
Akito914 1:3663d0f20136 135 linearInterpolation(currentPosition, nextPosition);
Akito914 1:3663d0f20136 136 currentPosition = nextPosition;
Akito914 1:3663d0f20136 137 wait_ms(100);
Akito914 1:3663d0f20136 138 nextPosition.x = 0; nextPosition.y = 200;
Akito914 1:3663d0f20136 139 linearInterpolation(currentPosition, nextPosition);
Akito914 1:3663d0f20136 140 currentPosition = nextPosition;
Akito914 1:3663d0f20136 141 wait_ms(100);
Akito914 1:3663d0f20136 142 nextPosition.x = 0; nextPosition.y = 0;
Akito914 1:3663d0f20136 143 linearInterpolation(currentPosition, nextPosition);
Akito914 1:3663d0f20136 144 currentPosition = nextPosition;
Akito914 1:3663d0f20136 145 wait_ms(100);
Akito914 1:3663d0f20136 146 }
Akito914 1:3663d0f20136 147 */
Akito914 1:3663d0f20136 148
Akito914 1:3663d0f20136 149 sendRequest();
Akito914 0:0eddebdc2c58 150
Akito914 0:0eddebdc2c58 151 while(1) {
Akito914 3:ddb5ca27e94b 152 myled = 1;
Akito914 1:3663d0f20136 153 while(job == 0);
Akito914 3:ddb5ca27e94b 154 myled = 0;
Akito914 1:3663d0f20136 155
Akito914 1:3663d0f20136 156 if(nextMode != 0){ // move mode
Akito914 2:80eb19e9091e 157 runningPosition = nextPosition;
Akito914 1:3663d0f20136 158 job = 0;
Akito914 1:3663d0f20136 159 sendRequest();
Akito914 2:80eb19e9091e 160 linearInterpolation(currentPosition, runningPosition);
Akito914 2:80eb19e9091e 161 currentPosition = runningPosition;
Akito914 2:80eb19e9091e 162 wait_ms(10);
Akito914 0:0eddebdc2c58 163 }
Akito914 1:3663d0f20136 164 else{ // pen mode
Akito914 2:80eb19e9091e 165 runningPenMotion = penMotion;
Akito914 2:80eb19e9091e 166 job = 0;
Akito914 2:80eb19e9091e 167 sendRequest();
Akito914 2:80eb19e9091e 168 if(runningPenMotion != 0){
Akito914 1:3663d0f20136 169 driveServo(PEN_DOWN);
Akito914 2:80eb19e9091e 170 wait_ms(100);
Akito914 1:3663d0f20136 171 }
Akito914 1:3663d0f20136 172 else{
Akito914 1:3663d0f20136 173 driveServo(PEN_UP);
Akito914 2:80eb19e9091e 174 wait_ms(100);
Akito914 1:3663d0f20136 175 }
Akito914 1:3663d0f20136 176 }
Akito914 1:3663d0f20136 177
Akito914 0:0eddebdc2c58 178 }
Akito914 0:0eddebdc2c58 179 }
Akito914 0:0eddebdc2c58 180
Akito914 0:0eddebdc2c58 181 void linearInterpolation(coordinate_t cStart, coordinate_t cTarg)
Akito914 0:0eddebdc2c58 182 {
Akito914 0:0eddebdc2c58 183 coordinate_t delta;
Akito914 0:0eddebdc2c58 184 int32_t error = 0;
Akito914 0:0eddebdc2c58 185 bool x_inv = false;
Akito914 0:0eddebdc2c58 186 bool y_inv = false;
Akito914 0:0eddebdc2c58 187 coordinate_t cCurrent = {cStart.x, cStart.y};
Akito914 0:0eddebdc2c58 188 coordinate_t inc = {0, 0};
Akito914 0:0eddebdc2c58 189 uint32_t period_us = 0;
Akito914 0:0eddebdc2c58 190
Akito914 0:0eddebdc2c58 191 if(cStart.x > cTarg.x) {
Akito914 0:0eddebdc2c58 192 x_inv = true;
Akito914 0:0eddebdc2c58 193 cTarg.x = cStart.x * 2 - cTarg.x;
Akito914 0:0eddebdc2c58 194 }
Akito914 0:0eddebdc2c58 195 if(cStart.y > cTarg.y) {
Akito914 0:0eddebdc2c58 196 y_inv = true;
Akito914 0:0eddebdc2c58 197 cTarg.y = cStart.y * 2 - cTarg.y;
Akito914 0:0eddebdc2c58 198 }
Akito914 1:3663d0f20136 199
Akito914 0:0eddebdc2c58 200 delta.x = cTarg.x - cStart.x;
Akito914 0:0eddebdc2c58 201 delta.y = cTarg.y - cStart.y;
Akito914 0:0eddebdc2c58 202
Akito914 2:80eb19e9091e 203 if((delta.x == 0)&&(delta.y == 0)){
Akito914 2:80eb19e9091e 204 return;
Akito914 2:80eb19e9091e 205 }
Akito914 2:80eb19e9091e 206
Akito914 0:0eddebdc2c58 207 if(delta.x < delta.y){
Akito914 0:0eddebdc2c58 208 motionCal.setTarg(delta.y);
Akito914 0:0eddebdc2c58 209 }
Akito914 0:0eddebdc2c58 210 else{
Akito914 0:0eddebdc2c58 211 motionCal.setTarg(delta.x);
Akito914 0:0eddebdc2c58 212 }
Akito914 0:0eddebdc2c58 213
Akito914 0:0eddebdc2c58 214 while(true) {
Akito914 0:0eddebdc2c58 215 inc.x = 0;
Akito914 0:0eddebdc2c58 216 inc.y = 0;
Akito914 1:3663d0f20136 217 if(delta.x == 0){
Akito914 2:80eb19e9091e 218 if(cCurrent.y < cTarg.y){
Akito914 2:80eb19e9091e 219 cCurrent.y++;
Akito914 2:80eb19e9091e 220 inc.y = 1;
Akito914 2:80eb19e9091e 221 }
Akito914 2:80eb19e9091e 222 else{
Akito914 2:80eb19e9091e 223 debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
Akito914 1:3663d0f20136 224 break;
Akito914 1:3663d0f20136 225 }
Akito914 1:3663d0f20136 226 period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
Akito914 1:3663d0f20136 227 }
Akito914 1:3663d0f20136 228 else if(delta.y == 0){
Akito914 2:80eb19e9091e 229 if(cCurrent.x < cTarg.x){
Akito914 2:80eb19e9091e 230 cCurrent.x++;
Akito914 2:80eb19e9091e 231 inc.x = 1;
Akito914 2:80eb19e9091e 232 }
Akito914 2:80eb19e9091e 233 else{
Akito914 2:80eb19e9091e 234 debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
Akito914 1:3663d0f20136 235 break;
Akito914 1:3663d0f20136 236 }
Akito914 1:3663d0f20136 237 period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
Akito914 1:3663d0f20136 238 }
Akito914 2:80eb19e9091e 239 else if(delta.y > delta.x) {
Akito914 2:80eb19e9091e 240 if(cCurrent.y < cTarg.y){
Akito914 2:80eb19e9091e 241 cCurrent.y++;
Akito914 2:80eb19e9091e 242 inc.y = 1;
Akito914 2:80eb19e9091e 243 }
Akito914 2:80eb19e9091e 244 else{
Akito914 2:80eb19e9091e 245 debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
Akito914 2:80eb19e9091e 246 break;
Akito914 2:80eb19e9091e 247 }
Akito914 0:0eddebdc2c58 248 error += delta.x;
Akito914 2:80eb19e9091e 249 if(error >= delta.y) {
Akito914 0:0eddebdc2c58 250 error -= delta.y;
Akito914 0:0eddebdc2c58 251 cCurrent.x++;
Akito914 0:0eddebdc2c58 252 inc.x = 1;
Akito914 0:0eddebdc2c58 253 }
Akito914 2:80eb19e9091e 254 period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
Akito914 2:80eb19e9091e 255 } else {
Akito914 2:80eb19e9091e 256 if(cCurrent.x < cTarg.x){
Akito914 2:80eb19e9091e 257 cCurrent.x++;
Akito914 2:80eb19e9091e 258 inc.x = 1;
Akito914 2:80eb19e9091e 259 }
Akito914 2:80eb19e9091e 260 else{
Akito914 2:80eb19e9091e 261 debugPort.printf("%d, %d : %d\r\n", cCurrent.x, cCurrent.y, error);
Akito914 1:3663d0f20136 262 break;
Akito914 1:3663d0f20136 263 }
Akito914 0:0eddebdc2c58 264 error += delta.y;
Akito914 2:80eb19e9091e 265 if(error >= delta.x) {
Akito914 0:0eddebdc2c58 266 error -= delta.x;
Akito914 0:0eddebdc2c58 267 cCurrent.y++;
Akito914 0:0eddebdc2c58 268 inc.y = 1;
Akito914 0:0eddebdc2c58 269 }
Akito914 0:0eddebdc2c58 270 period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
Akito914 0:0eddebdc2c58 271 }
Akito914 0:0eddebdc2c58 272
Akito914 0:0eddebdc2c58 273 if(inc.x != 0)x_axis.oneStep(x_inv == 0);
Akito914 0:0eddebdc2c58 274 if(inc.y != 0)y_axis.oneStep(y_inv == 0);
Akito914 0:0eddebdc2c58 275
Akito914 0:0eddebdc2c58 276 wait_us(period_us);
Akito914 1:3663d0f20136 277
Akito914 0:0eddebdc2c58 278 }
Akito914 0:0eddebdc2c58 279 }
Akito914 0:0eddebdc2c58 280
Akito914 0:0eddebdc2c58 281 void trapezoid(int32_t targSteps)
Akito914 0:0eddebdc2c58 282 {
Akito914 0:0eddebdc2c58 283 int revMode = 0;
Akito914 0:0eddebdc2c58 284
Akito914 0:0eddebdc2c58 285 uint32_t period_us = 0;
Akito914 0:0eddebdc2c58 286 uint32_t steps = 0;
Akito914 0:0eddebdc2c58 287
Akito914 0:0eddebdc2c58 288 if(targSteps < 0) {
Akito914 0:0eddebdc2c58 289 revMode = 1;
Akito914 0:0eddebdc2c58 290 targSteps *= -1;
Akito914 0:0eddebdc2c58 291 }
Akito914 0:0eddebdc2c58 292
Akito914 0:0eddebdc2c58 293 motionCal.setTarg(targSteps);
Akito914 0:0eddebdc2c58 294
Akito914 0:0eddebdc2c58 295 while (steps <= targSteps) {
Akito914 0:0eddebdc2c58 296
Akito914 0:0eddebdc2c58 297 period_us = motionCal.calStepDelay(steps);
Akito914 0:0eddebdc2c58 298
Akito914 0:0eddebdc2c58 299 steps += 1;
Akito914 0:0eddebdc2c58 300
Akito914 0:0eddebdc2c58 301 x_axis.oneStep(revMode != 1);
Akito914 0:0eddebdc2c58 302
Akito914 0:0eddebdc2c58 303 wait_us(period_us);
Akito914 0:0eddebdc2c58 304
Akito914 0:0eddebdc2c58 305 }
Akito914 1:3663d0f20136 306 }
Akito914 0:0eddebdc2c58 307
Akito914 1:3663d0f20136 308
Akito914 1:3663d0f20136 309 void beep(int T_us,int t_ms){
Akito914 1:3663d0f20136 310
Akito914 1:3663d0f20136 311 if(T_us==0 || t_ms==0)return;
Akito914 1:3663d0f20136 312
Akito914 1:3663d0f20136 313 speaker.period_us(T_us);
Akito914 1:3663d0f20136 314 speaker.write(0.10);
Akito914 1:3663d0f20136 315
Akito914 1:3663d0f20136 316 wait_ms(t_ms);
Akito914 1:3663d0f20136 317
Akito914 1:3663d0f20136 318 speaker.write(0.0);
Akito914 1:3663d0f20136 319 speaker.period_us(100);
Akito914 1:3663d0f20136 320
Akito914 1:3663d0f20136 321 return;
Akito914 0:0eddebdc2c58 322 }
Akito914 1:3663d0f20136 323
Akito914 1:3663d0f20136 324 void pi(int times){
Akito914 1:3663d0f20136 325 int count=0;
Akito914 1:3663d0f20136 326
Akito914 1:3663d0f20136 327 for(count=0;count<times;count++){
Akito914 1:3663d0f20136 328 beep(379,50);
Akito914 1:3663d0f20136 329 wait_ms(50);
Akito914 1:3663d0f20136 330 }
Akito914 1:3663d0f20136 331 wait_ms(300);
Akito914 1:3663d0f20136 332 }
Akito914 1:3663d0f20136 333
Akito914 1:3663d0f20136 334 void q_sound(void){
Akito914 1:3663d0f20136 335 beep(478,100);
Akito914 1:3663d0f20136 336 beep(379,100);
Akito914 1:3663d0f20136 337 }
Akito914 1:3663d0f20136 338
Akito914 1:3663d0f20136 339 void ans_sound(void){
Akito914 1:3663d0f20136 340 beep(379,100);
Akito914 1:3663d0f20136 341 beep(478,100);
Akito914 1:3663d0f20136 342 }
Akito914 1:3663d0f20136 343
Akito914 1:3663d0f20136 344 void piroro(void){
Akito914 1:3663d0f20136 345 beep(379,100);
Akito914 1:3663d0f20136 346 beep(426,100);
Akito914 1:3663d0f20136 347 beep(478,100);
Akito914 1:3663d0f20136 348 }
Akito914 1:3663d0f20136 349
Akito914 1:3663d0f20136 350
Akito914 1:3663d0f20136 351
Akito914 1:3663d0f20136 352
Akito914 1:3663d0f20136 353