Akito / Mbed 2 deprecated kisoken_PenPlotter

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

Dependencies:   SPM_simple mbed trapezoidalMotionCal

main.cpp@5:0abb848bb2f9, 2017-09-07 (annotated)

Committer:
Akito914
Date:
Thu Sep 07 06:43:22 2017 +0000
Revision:
5:0abb848bb2f9
Parent:
4:76146d1e248a 
???????????????????

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