Akito / Mbed 2 deprecated kisoken_PenPlotter

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

Dependencies:   SPM_simple mbed trapezoidalMotionCal

main.cpp@1:3663d0f20136, 2017-08-03 (annotated)

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