Wansik Choi / Mbed 2 deprecated linear_motor

linear motor code for ksk

Dependencies:   mbed

main.cpp@2:81a944155520, 2017-02-25 (annotated)

Committer:
cws8262
Date:
Sat Feb 25 08:14:13 2017 +0000
Revision:
2:81a944155520
Parent:
1:ff295adcca59 
ds

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cws8262 0:bc6aa7a92c1b 1 #include "mbed.h"
cws8262 0:bc6aa7a92c1b 2
cws8262 0:bc6aa7a92c1b 3 #include "LinearMotor.h"
cws8262 1:ff295adcca59 4
cws8262 1:ff295adcca59 5 int ItvTable[200]= {5000,3219,2373,1880,1556,1327,1157,1026,921,836,765,706,654,610,572,538,507,480,456,434,414,396,380,364,350,337,325,314,303,293,284,275,267,260,252,246,239,233,227,221,216,211,206,202,197,193,189,185,181,178,174,171,168,165,162,159,156,154,151,149,146,144,142,139,137,135,133,131,129,128,126,124,122,121,119,118,116,115,113,112,110,109,108,107,105,104,103,102,101,100,98,97,96,95,94,93,92,91,91,90,89,88,87,86,85,85,84,83,82,82,81,80,79,79,78,77,77,76,75,75,74,74,73,72,72,71,71,70,70,69,69,68,68,67,67,66,66,65,65,64,64,63,63,62,62,62,61,61,60,60,60,59,59,58,58,58,57,57,57,56,56,56,55,55,54,54,54,54,53,53,53,52,52,52,51,51,51,51,50,50,50,49,49,49,49,48,48,48,48,47,47,47,47,46,46,46,46,45,45,45};
cws8262 1:ff295adcca59 6 int stepTable[200]= {2,5,9,14,20,28,37,47,58,70,83,97,112,128,145,164,184,205,227,250,274,299,325,352,381,411,442,474,507,541,576,612,649,687,727,768,810,853,897,942,988,1035,1084,1134,1185,1237,1290,1344,1399,1455,1512,1570,1630,1691,1753,1816,1880,1945,2011,2078,2146,2215,2285,2357,2430,2504,2579,2655,2733,2811,2890,2971,3053,3136,3220,3305,3391,3478,3566,3655,3746,3838,3931,4024,4119,4215,4312,4410,4509,4609,4711,4814,4918,5023,5129,5237,5346,5456,5566,5677,5789,5903,6018,6134,6252,6370,6489,6609,6731,6853,6976,7101,7228,7355,7483,7613,7743,7875,8008,8141,8276,8411,8548,8687,8826,8967,9108,9251,9394,9539,9684,9831,9978,10127,10276,10428,10580,10734,10888,11044,11200,11359,11518,11679,11840,12001,12165,12329,12496,12663,12830,12999,13168,13340,13512,13684,13859,14034,14209,14388,14567,14746,14928,15110,15295,15480,15665,15850,16039,16228,16417,16609,16801,16993,17189,17385,17581,17777,17977,18177,18377,18581,18785,18989,19193,19401,19609,19817,20025,20238,20451,20664,20877,21094,21311,21528,21745,21967,22189,22411};
cws8262 1:ff295adcca59 7
cws8262 1:ff295adcca59 8
cws8262 1:ff295adcca59 9 Serial pc(SERIAL_TX, SERIAL_RX);
cws8262 0:bc6aa7a92c1b 10
cws8262 1:ff295adcca59 11 //0.00225mm/step
cws8262 1:ff295adcca59 12 //500usec=4.5mm/sec
cws8262 1:ff295adcca59 13 //45usec=50mm/sec
cws8262 1:ff295adcca59 14 LinearMotor lm(PA_7,PA_6,PA_5,PA_0);
cws8262 1:ff295adcca59 15 void move(double dis, double spd);
cws8262 1:ff295adcca59 16 void init(void);
cws8262 1:ff295adcca59 17
cws8262 2:81a944155520 18 void pause(void);
cws8262 2:81a944155520 19 InterruptIn psw(PA_8);
cws8262 2:81a944155520 20 DigitalIn psw_(PA_8);
cws8262 1:ff295adcca59 21 int main() {
cws8262 2:81a944155520 22
cws8262 2:81a944155520 23 psw_.mode(PullUp);
cws8262 2:81a944155520 24 pc.baud(230400);
cws8262 2:81a944155520 25 if(psw_.read()) pc.printf("\n\rPause.... ");
cws8262 2:81a944155520 26 while(psw_.read());
cws8262 2:81a944155520 27 pc.printf("Start");
cws8262 2:81a944155520 28 wait_ms(20);
cws8262 1:ff295adcca59 29
cws8262 2:81a944155520 30 psw.rise(&pause);
cws8262 1:ff295adcca59 31 lm.st=1;
cws8262 1:ff295adcca59 32 wait_ms(500);
cws8262 1:ff295adcca59 33 init();
cws8262 1:ff295adcca59 34 wait_ms(500);
cws8262 0:bc6aa7a92c1b 35 while(1) {
cws8262 1:ff295adcca59 36 move(50,10);
cws8262 1:ff295adcca59 37 wait_ms(200);
cws8262 1:ff295adcca59 38 move(-50,10);
cws8262 1:ff295adcca59 39 wait_ms(200);
cws8262 0:bc6aa7a92c1b 40 }
cws8262 0:bc6aa7a92c1b 41 }
cws8262 1:ff295adcca59 42
cws8262 1:ff295adcca59 43
cws8262 1:ff295adcca59 44 void move(double dis, double spd){//spd mm/sec
cws8262 1:ff295adcca59 45 pc.printf("Moving %10fmm at %10fmm/sec... ",dis,spd);
cws8262 1:ff295adcca59 46 int j=0;
cws8262 1:ff295adcca59 47 int crntDly=5000;
cws8262 1:ff295adcca59 48 int i;
cws8262 1:ff295adcca59 49
cws8262 1:ff295adcca59 50 if (dis>0)lm.dir=1;
cws8262 1:ff295adcca59 51 else{
cws8262 1:ff295adcca59 52 lm.dir=0;
cws8262 1:ff295adcca59 53 dis=-dis;
cws8262 1:ff295adcca59 54 }
cws8262 1:ff295adcca59 55
cws8262 1:ff295adcca59 56 int stepDly=(int)(0.5+2250/spd);
cws8262 1:ff295adcca59 57 int step=(int)(0.5+dis/0.00225);
cws8262 1:ff295adcca59 58 if (stepDly>=ItvTable[0]){
cws8262 1:ff295adcca59 59 for(i=0;i<step;i++){
cws8262 1:ff295adcca59 60 lm.pulse=!lm.pulse;
cws8262 1:ff295adcca59 61 wait_us(stepDly);
cws8262 1:ff295adcca59 62 }
cws8262 1:ff295adcca59 63 }
cws8262 1:ff295adcca59 64 else{
cws8262 1:ff295adcca59 65 int ihigh=0;
cws8262 1:ff295adcca59 66 for(i=0;i<(int)(step/2);i++){
cws8262 1:ff295adcca59 67 lm.pulse=!lm.pulse;
cws8262 1:ff295adcca59 68 if(j>=200 || crntDly<=stepDly){crntDly=stepDly;}
cws8262 1:ff295adcca59 69 else if(i<stepTable[j]){crntDly=ItvTable[j];}
cws8262 1:ff295adcca59 70 else {j++;ihigh=i;}
cws8262 1:ff295adcca59 71 wait_us(crntDly);
cws8262 1:ff295adcca59 72 }
cws8262 1:ff295adcca59 73 for(i=(int)(step/2);i>0;i--){
cws8262 1:ff295adcca59 74 lm.pulse=!lm.pulse;
cws8262 1:ff295adcca59 75 if(i>=ihigh){crntDly=stepDly;}
cws8262 1:ff295adcca59 76 else if(j<=0 || crntDly>=ItvTable[0]){crntDly=ItvTable[0];}
cws8262 1:ff295adcca59 77 else if(i>=stepTable[j]){crntDly=ItvTable[j];}
cws8262 1:ff295adcca59 78 else {j--;}
cws8262 1:ff295adcca59 79 wait_us(crntDly);
cws8262 1:ff295adcca59 80 }
cws8262 1:ff295adcca59 81 }
cws8262 1:ff295adcca59 82 pc.printf("Complete\n\r");
cws8262 1:ff295adcca59 83 }
cws8262 1:ff295adcca59 84
cws8262 1:ff295adcca59 85
cws8262 1:ff295adcca59 86 void init(void){
cws8262 1:ff295adcca59 87 lm.dir=0;
cws8262 1:ff295adcca59 88 int i=0;
cws8262 1:ff295adcca59 89 int j=0;
cws8262 1:ff295adcca59 90 int crntDly=5000;
cws8262 1:ff295adcca59 91 int stepDly=(int)(0.5+2250/10);
cws8262 1:ff295adcca59 92 pc.printf("\n\rInitializing... ");
cws8262 1:ff295adcca59 93 while(lm.limit==0){
cws8262 1:ff295adcca59 94 lm.pulse=!lm.pulse;
cws8262 1:ff295adcca59 95 if(j>=200 || crntDly<=stepDly){crntDly=stepDly;}
cws8262 1:ff295adcca59 96 else if(i<stepTable[j]){crntDly=ItvTable[j];}
cws8262 1:ff295adcca59 97 else {j++;}
cws8262 1:ff295adcca59 98 wait_us(crntDly);
cws8262 1:ff295adcca59 99 i++;
cws8262 1:ff295adcca59 100 }
cws8262 1:ff295adcca59 101 pc.printf("Complete\n\r");
cws8262 2:81a944155520 102 }
cws8262 2:81a944155520 103
cws8262 2:81a944155520 104 void pause(void){
cws8262 2:81a944155520 105 psw.disable_irq();
cws8262 2:81a944155520 106 wait_ms(20);
cws8262 2:81a944155520 107 pc.printf("\n\rPause....");
cws8262 2:81a944155520 108 while(psw_.read());
cws8262 2:81a944155520 109 pc.printf(" Resume\n\r");
cws8262 2:81a944155520 110 psw.enable_irq();
cws8262 1:ff295adcca59 111 }