Akito / Mbed 2 deprecated kisoken_PenPlotter

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

Dependencies:   SPM_simple mbed trapezoidalMotionCal

main.cpp@0:0eddebdc2c58, 2017-08-03 (annotated)

Committer:
Akito914
Date:
Thu Aug 03 06:51:15 2017 +0000
Revision:
0:0eddebdc2c58
Child:
1:3663d0f20136
????????????????;

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 0:0eddebdc2c58 5 //stepMotor x_axis(PC_4 ,PB_13 ,PB_14 ,PB_15);
Akito914 0:0eddebdc2c58 6 stepMotor x_axis(PB_15 ,PB_14 ,PB_13 ,PC_4);
Akito914 0:0eddebdc2c58 7 stepMotor y_axis(PB_1 ,PB_2 ,PB_12 ,PA_11);
Akito914 0:0eddebdc2c58 8
Akito914 0:0eddebdc2c58 9 trapezoidalMotionCal motionCal(200, 200, 4000, 40000, -40000);
Akito914 0:0eddebdc2c58 10
Akito914 0:0eddebdc2c58 11 PwmOut servo(PC_6);
Akito914 0:0eddebdc2c58 12
Akito914 0:0eddebdc2c58 13 DigitalIn x_originSW(PA_0);
Akito914 0:0eddebdc2c58 14 DigitalIn y_originSW(PA_1);
Akito914 0:0eddebdc2c58 15
Akito914 0:0eddebdc2c58 16 DigitalOut myled(LED1);
Akito914 0:0eddebdc2c58 17 Serial pc(USBTX,USBRX);
Akito914 0:0eddebdc2c58 18
Akito914 0:0eddebdc2c58 19 typedef struct {
Akito914 0:0eddebdc2c58 20 int32_t x;
Akito914 0:0eddebdc2c58 21 int32_t y;
Akito914 0:0eddebdc2c58 22 } coordinate_t;
Akito914 0:0eddebdc2c58 23
Akito914 0:0eddebdc2c58 24 int task = 0;
Akito914 0:0eddebdc2c58 25 int32_t pulse = 0;
Akito914 0:0eddebdc2c58 26
Akito914 0:0eddebdc2c58 27
Akito914 0:0eddebdc2c58 28
Akito914 0:0eddebdc2c58 29 void pc_intr();
Akito914 0:0eddebdc2c58 30 void driveServo(int deg);
Akito914 0:0eddebdc2c58 31 void linearInterpolation(coordinate_t cStart, coordinate_t cTarg);
Akito914 0:0eddebdc2c58 32 void trapezoid(int32_t targSteps);
Akito914 0:0eddebdc2c58 33
Akito914 0:0eddebdc2c58 34 // 受信割込みハンドラ
Akito914 0:0eddebdc2c58 35 void pc_intr()
Akito914 0:0eddebdc2c58 36 {
Akito914 0:0eddebdc2c58 37 char temp = pc.getc();
Akito914 0:0eddebdc2c58 38
Akito914 0:0eddebdc2c58 39 switch(temp) {
Akito914 0:0eddebdc2c58 40
Akito914 0:0eddebdc2c58 41 case ',' :
Akito914 0:0eddebdc2c58 42 if(task==0) {
Akito914 0:0eddebdc2c58 43 task=1;
Akito914 0:0eddebdc2c58 44 pulse=200;
Akito914 0:0eddebdc2c58 45 }
Akito914 0:0eddebdc2c58 46 break;
Akito914 0:0eddebdc2c58 47 case '.' :
Akito914 0:0eddebdc2c58 48 if(task==0) {
Akito914 0:0eddebdc2c58 49 task=1;
Akito914 0:0eddebdc2c58 50 pulse=-200;
Akito914 0:0eddebdc2c58 51 }
Akito914 0:0eddebdc2c58 52 break;
Akito914 0:0eddebdc2c58 53 case 's' :
Akito914 0:0eddebdc2c58 54 driveServo(120);
Akito914 0:0eddebdc2c58 55 break;
Akito914 0:0eddebdc2c58 56 case 'u' :
Akito914 0:0eddebdc2c58 57 driveServo(100);
Akito914 0:0eddebdc2c58 58 break;
Akito914 0:0eddebdc2c58 59 case 'd' :
Akito914 0:0eddebdc2c58 60 driveServo(60);
Akito914 0:0eddebdc2c58 61 break;
Akito914 0:0eddebdc2c58 62
Akito914 0:0eddebdc2c58 63 }
Akito914 0:0eddebdc2c58 64 }
Akito914 0:0eddebdc2c58 65
Akito914 0:0eddebdc2c58 66 void driveServo(int deg){
Akito914 0:0eddebdc2c58 67 const static int servo_offset =830;
Akito914 0:0eddebdc2c58 68 const static float servo_gain =10.0;
Akito914 0:0eddebdc2c58 69
Akito914 0:0eddebdc2c58 70 deg=deg*servo_gain+servo_offset;
Akito914 0:0eddebdc2c58 71 servo.pulsewidth_us(deg);
Akito914 0:0eddebdc2c58 72
Akito914 0:0eddebdc2c58 73 return;
Akito914 0:0eddebdc2c58 74 }
Akito914 0:0eddebdc2c58 75
Akito914 0:0eddebdc2c58 76 void itp(int32_t x, int32_t y){
Akito914 0:0eddebdc2c58 77 static coordinate_t cPast = {0,0};
Akito914 0:0eddebdc2c58 78 coordinate_t cCurrent = {x,y};
Akito914 0:0eddebdc2c58 79
Akito914 0:0eddebdc2c58 80 linearInterpolation(cPast, cCurrent);
Akito914 0:0eddebdc2c58 81
Akito914 0:0eddebdc2c58 82 cPast.x = x;
Akito914 0:0eddebdc2c58 83 cPast.y = y;
Akito914 0:0eddebdc2c58 84 }
Akito914 0:0eddebdc2c58 85
Akito914 0:0eddebdc2c58 86 int main()
Akito914 0:0eddebdc2c58 87 {
Akito914 0:0eddebdc2c58 88 pc.attach(pc_intr, Serial::RxIrq);
Akito914 0:0eddebdc2c58 89
Akito914 0:0eddebdc2c58 90 pc.printf("Hello\r\n");
Akito914 0:0eddebdc2c58 91 /*
Akito914 0:0eddebdc2c58 92 x_axis.lock();
Akito914 0:0eddebdc2c58 93 y_axis.lock();
Akito914 0:0eddebdc2c58 94 */
Akito914 0:0eddebdc2c58 95 driveServo(120);
Akito914 0:0eddebdc2c58 96
Akito914 0:0eddebdc2c58 97 while(1) {
Akito914 0:0eddebdc2c58 98
Akito914 0:0eddebdc2c58 99 if(task==1) {
Akito914 0:0eddebdc2c58 100
Akito914 0:0eddebdc2c58 101 myled = 1;
Akito914 0:0eddebdc2c58 102
Akito914 0:0eddebdc2c58 103 itp(400, 0);
Akito914 0:0eddebdc2c58 104 wait_ms(10);
Akito914 0:0eddebdc2c58 105 itp(400,400);
Akito914 0:0eddebdc2c58 106 wait_ms(10);
Akito914 0:0eddebdc2c58 107 itp( 0,400);
Akito914 0:0eddebdc2c58 108 wait_ms(10);
Akito914 0:0eddebdc2c58 109 itp( 0, 0);
Akito914 0:0eddebdc2c58 110 wait_ms(10);
Akito914 0:0eddebdc2c58 111 itp(400, 0);
Akito914 0:0eddebdc2c58 112 wait_ms(10);
Akito914 0:0eddebdc2c58 113 itp( 0,400);
Akito914 0:0eddebdc2c58 114 wait_ms(10);
Akito914 0:0eddebdc2c58 115 itp(400,400);
Akito914 0:0eddebdc2c58 116 wait_ms(10);
Akito914 0:0eddebdc2c58 117 itp( 0, 0);
Akito914 0:0eddebdc2c58 118 wait_ms(10);
Akito914 0:0eddebdc2c58 119
Akito914 0:0eddebdc2c58 120 myled = 0;
Akito914 0:0eddebdc2c58 121
Akito914 0:0eddebdc2c58 122 task=0;
Akito914 0:0eddebdc2c58 123 }
Akito914 0:0eddebdc2c58 124
Akito914 0:0eddebdc2c58 125 }
Akito914 0:0eddebdc2c58 126 }
Akito914 0:0eddebdc2c58 127
Akito914 0:0eddebdc2c58 128 void linearInterpolation(coordinate_t cStart, coordinate_t cTarg)
Akito914 0:0eddebdc2c58 129 {
Akito914 0:0eddebdc2c58 130 coordinate_t delta;
Akito914 0:0eddebdc2c58 131 int32_t error = 0;
Akito914 0:0eddebdc2c58 132 bool x_inv = false;
Akito914 0:0eddebdc2c58 133 bool y_inv = false;
Akito914 0:0eddebdc2c58 134 coordinate_t cCurrent = {cStart.x, cStart.y};
Akito914 0:0eddebdc2c58 135 coordinate_t inc = {0, 0};
Akito914 0:0eddebdc2c58 136 uint32_t period_us = 0;
Akito914 0:0eddebdc2c58 137
Akito914 0:0eddebdc2c58 138 if(cStart.x > cTarg.x) {
Akito914 0:0eddebdc2c58 139 x_inv = true;
Akito914 0:0eddebdc2c58 140 cTarg.x = cStart.x * 2 - cTarg.x;
Akito914 0:0eddebdc2c58 141 }
Akito914 0:0eddebdc2c58 142 if(cStart.y > cTarg.y) {
Akito914 0:0eddebdc2c58 143 y_inv = true;
Akito914 0:0eddebdc2c58 144 cTarg.y = cStart.y * 2 - cTarg.y;
Akito914 0:0eddebdc2c58 145 }
Akito914 0:0eddebdc2c58 146
Akito914 0:0eddebdc2c58 147 delta.x = cTarg.x - cStart.x;
Akito914 0:0eddebdc2c58 148 delta.y = cTarg.y - cStart.y;
Akito914 0:0eddebdc2c58 149
Akito914 0:0eddebdc2c58 150 if(delta.x < delta.y){
Akito914 0:0eddebdc2c58 151 motionCal.setTarg(delta.y);
Akito914 0:0eddebdc2c58 152 }
Akito914 0:0eddebdc2c58 153 else{
Akito914 0:0eddebdc2c58 154 motionCal.setTarg(delta.x);
Akito914 0:0eddebdc2c58 155 }
Akito914 0:0eddebdc2c58 156
Akito914 0:0eddebdc2c58 157 while(true) {
Akito914 0:0eddebdc2c58 158 inc.x = 0;
Akito914 0:0eddebdc2c58 159 inc.y = 0;
Akito914 0:0eddebdc2c58 160 if(delta.y > delta.x) {
Akito914 0:0eddebdc2c58 161 error += delta.x;
Akito914 0:0eddebdc2c58 162 if(error > delta.y) {
Akito914 0:0eddebdc2c58 163 error -= delta.y;
Akito914 0:0eddebdc2c58 164 cCurrent.x++;
Akito914 0:0eddebdc2c58 165 inc.x = 1;
Akito914 0:0eddebdc2c58 166 }
Akito914 0:0eddebdc2c58 167 if(cCurrent.y == cTarg.y)break;
Akito914 0:0eddebdc2c58 168 cCurrent.y++;
Akito914 0:0eddebdc2c58 169 inc.y = 1;
Akito914 0:0eddebdc2c58 170 period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
Akito914 0:0eddebdc2c58 171 } else {
Akito914 0:0eddebdc2c58 172 error += delta.y;
Akito914 0:0eddebdc2c58 173 if(error > delta.x) {
Akito914 0:0eddebdc2c58 174 error -= delta.x;
Akito914 0:0eddebdc2c58 175 cCurrent.y++;
Akito914 0:0eddebdc2c58 176 inc.y = 1;
Akito914 0:0eddebdc2c58 177 }
Akito914 0:0eddebdc2c58 178 if(cCurrent.x == cTarg.x)break;
Akito914 0:0eddebdc2c58 179 cCurrent.x++;
Akito914 0:0eddebdc2c58 180 inc.x = 1;
Akito914 0:0eddebdc2c58 181 period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
Akito914 0:0eddebdc2c58 182 }
Akito914 0:0eddebdc2c58 183
Akito914 0:0eddebdc2c58 184 if(inc.x != 0)x_axis.oneStep(x_inv == 0);
Akito914 0:0eddebdc2c58 185 if(inc.y != 0)y_axis.oneStep(y_inv == 0);
Akito914 0:0eddebdc2c58 186
Akito914 0:0eddebdc2c58 187 wait_us(period_us);
Akito914 0:0eddebdc2c58 188
Akito914 0:0eddebdc2c58 189 }
Akito914 0:0eddebdc2c58 190
Akito914 0:0eddebdc2c58 191 }
Akito914 0:0eddebdc2c58 192
Akito914 0:0eddebdc2c58 193 void trapezoid(int32_t targSteps)
Akito914 0:0eddebdc2c58 194 {
Akito914 0:0eddebdc2c58 195 int revMode = 0;
Akito914 0:0eddebdc2c58 196
Akito914 0:0eddebdc2c58 197 uint32_t period_us = 0;
Akito914 0:0eddebdc2c58 198 uint32_t steps = 0;
Akito914 0:0eddebdc2c58 199
Akito914 0:0eddebdc2c58 200 if(targSteps < 0) {
Akito914 0:0eddebdc2c58 201 revMode = 1;
Akito914 0:0eddebdc2c58 202 targSteps *= -1;
Akito914 0:0eddebdc2c58 203 }
Akito914 0:0eddebdc2c58 204
Akito914 0:0eddebdc2c58 205 motionCal.setTarg(targSteps);
Akito914 0:0eddebdc2c58 206
Akito914 0:0eddebdc2c58 207 while (steps <= targSteps) {
Akito914 0:0eddebdc2c58 208
Akito914 0:0eddebdc2c58 209 period_us = motionCal.calStepDelay(steps);
Akito914 0:0eddebdc2c58 210
Akito914 0:0eddebdc2c58 211 steps += 1;
Akito914 0:0eddebdc2c58 212
Akito914 0:0eddebdc2c58 213 x_axis.oneStep(revMode != 1);
Akito914 0:0eddebdc2c58 214
Akito914 0:0eddebdc2c58 215 wait_us(period_us);
Akito914 0:0eddebdc2c58 216
Akito914 0:0eddebdc2c58 217 }
Akito914 0:0eddebdc2c58 218
Akito914 0:0eddebdc2c58 219 }