HSU CHI-LUN / sMotor0313

test0313

main_test_0314.cpp@2:9b9db9c1c097, 2019-10-17 (annotated)

Committer:
lpisuseful
Date:
Thu Oct 17 07:17:04 2019 +0000
Revision:
2:9b9db9c1c097
test1081017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lpisuseful 2:9b9db9c1c097 1 /*
lpisuseful 2:9b9db9c1c097 2 ############################################
lpisuseful 2:9b9db9c1c097 3 ## sMotor v0.1 Test Program ##
lpisuseful 2:9b9db9c1c097 4 ## created by Samuel Matildes ##
lpisuseful 2:9b9db9c1c097 5 ############################################
lpisuseful 2:9b9db9c1c097 6 ---- sam.naeec@gmail.com -----
lpisuseful 2:9b9db9c1c097 7 This library was made for 4-Phase Stepper Motors
lpisuseful 2:9b9db9c1c097 8 I don't take any resposability for the damage caused to your equipment.
lpisuseful 2:9b9db9c1c097 9
lpisuseful 2:9b9db9c1c097 10 */
lpisuseful 2:9b9db9c1c097 11
lpisuseful 2:9b9db9c1c097 12 #include "mbed.h"
lpisuseful 2:9b9db9c1c097 13 #include "sMotor.h"
lpisuseful 2:9b9db9c1c097 14 #include "UIPEthernet.h"
lpisuseful 2:9b9db9c1c097 15
lpisuseful 2:9b9db9c1c097 16
lpisuseful 2:9b9db9c1c097 17 Serial pc(USBTX, USBRX);
lpisuseful 2:9b9db9c1c097 18 sMotor motor(A0, A1, A2, A3); // set the pin sMotor motor(pin1--blueline ,pin2--pinkline ,pin3--yellowline ,pin4--orangeline)
lpisuseful 2:9b9db9c1c097 19 DigitalIn Button(D10);
lpisuseful 2:9b9db9c1c097 20 DigitalIn set_zero_point(D11);
lpisuseful 2:9b9db9c1c097 21 DigitalOut LED(D12);
lpisuseful 2:9b9db9c1c097 22
lpisuseful 2:9b9db9c1c097 23
lpisuseful 2:9b9db9c1c097 24
lpisuseful 2:9b9db9c1c097 25
lpisuseful 2:9b9db9c1c097 26 int step_speed = 1200 ; // set default motor speed
lpisuseful 2:9b9db9c1c097 27 int numstep = 512 ; // defines full turn of 360 degree
lpisuseful 2:9b9db9c1c097 28 int zerocount = 0 ;
lpisuseful 2:9b9db9c1c097 29 int old_zerocount = 0 ;
lpisuseful 2:9b9db9c1c097 30 int set_zero_step = 0 ;
lpisuseful 2:9b9db9c1c097 31
lpisuseful 2:9b9db9c1c097 32 //int delaytime = 1200;
lpisuseful 2:9b9db9c1c097 33 //you might want to calibrate this value according to your motor
lpisuseful 2:9b9db9c1c097 34
lpisuseful 2:9b9db9c1c097 35
lpisuseful 2:9b9db9c1c097 36
lpisuseful 2:9b9db9c1c097 37
lpisuseful 2:9b9db9c1c097 38 const uint8_t MY_MAC[6] = { 0x74,0x69,0x69,0x2D,0x30,0x31 };
lpisuseful 2:9b9db9c1c097 39 // IP address must be unique and compatible with your network.
lpisuseful 2:9b9db9c1c097 40 const IPAddress MY_IP(192, 168, 1, 181);
lpisuseful 2:9b9db9c1c097 41 const uint16_t MY_PORT = 7;
lpisuseful 2:9b9db9c1c097 42 const char* message = "Response successed!";
lpisuseful 2:9b9db9c1c097 43
lpisuseful 2:9b9db9c1c097 44
lpisuseful 2:9b9db9c1c097 45 UIPEthernet uIPEthernet(A6, A5, A4, D3); // mosi, miso, sck, cs
lpisuseful 2:9b9db9c1c097 46 UIPUDP udp;
lpisuseful 2:9b9db9c1c097 47
lpisuseful 2:9b9db9c1c097 48
lpisuseful 2:9b9db9c1c097 49
lpisuseful 2:9b9db9c1c097 50
lpisuseful 2:9b9db9c1c097 51
lpisuseful 2:9b9db9c1c097 52
lpisuseful 2:9b9db9c1c097 53 //////////////////////////////////////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 54
lpisuseful 2:9b9db9c1c097 55
lpisuseful 2:9b9db9c1c097 56 int b=0; //要到度數
lpisuseful 2:9b9db9c1c097 57 //const char* mesg;
lpisuseful 2:9b9db9c1c097 58 int e=0; //b的記憶體
lpisuseful 2:9b9db9c1c097 59 //const char* message[3];
lpisuseful 2:9b9db9c1c097 60
lpisuseful 2:9b9db9c1c097 61 int c=0; //現在整數步數
lpisuseful 2:9b9db9c1c097 62 int vardegree;//度數變化量
lpisuseful 2:9b9db9c1c097 63 int memory=1;//記憶度數
lpisuseful 2:9b9db9c1c097 64 int memorydegree=0;//記憶上次度數
lpisuseful 2:9b9db9c1c097 65
lpisuseful 2:9b9db9c1c097 66 void scan_int(int);//讀取函數
lpisuseful 2:9b9db9c1c097 67 int trans(int);//轉換函數
lpisuseful 2:9b9db9c1c097 68
lpisuseful 2:9b9db9c1c097 69
lpisuseful 2:9b9db9c1c097 70
lpisuseful 2:9b9db9c1c097 71 ///////////////////////////////////////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 72
lpisuseful 2:9b9db9c1c097 73
lpisuseful 2:9b9db9c1c097 74
lpisuseful 2:9b9db9c1c097 75 int main(void)
lpisuseful 2:9b9db9c1c097 76 {
lpisuseful 2:9b9db9c1c097 77
lpisuseful 2:9b9db9c1c097 78
lpisuseful 2:9b9db9c1c097 79 ////////////////////////////////////歸零///////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 80
lpisuseful 2:9b9db9c1c097 81 LED = 1 ; // if don't setzero LED is lighting
lpisuseful 2:9b9db9c1c097 82 printf("Haven't set_zero_point\n\r");
lpisuseful 2:9b9db9c1c097 83 pc.readable();
lpisuseful 2:9b9db9c1c097 84 motor.setzerostep(numstep,0,step_speed);
lpisuseful 2:9b9db9c1c097 85
lpisuseful 2:9b9db9c1c097 86 while(LED ==1) //LED歸零指式燈
lpisuseful 2:9b9db9c1c097 87 {
lpisuseful 2:9b9db9c1c097 88 if( Button == 1 && old_zerocount == zerocount) //按下按鈕進行手動歸零
lpisuseful 2:9b9db9c1c097 89 {
lpisuseful 2:9b9db9c1c097 90 motor.setzerostep(numstep,0,step_speed);
lpisuseful 2:9b9db9c1c097 91 }
lpisuseful 2:9b9db9c1c097 92 if(old_zerocount != zerocount)
lpisuseful 2:9b9db9c1c097 93 {
lpisuseful 2:9b9db9c1c097 94 printf("Press the botton to set the zero point \n\r");
lpisuseful 2:9b9db9c1c097 95 old_zerocount = zerocount;
lpisuseful 2:9b9db9c1c097 96 }
lpisuseful 2:9b9db9c1c097 97 }
lpisuseful 2:9b9db9c1c097 98
lpisuseful 2:9b9db9c1c097 99
lpisuseful 2:9b9db9c1c097 100
lpisuseful 2:9b9db9c1c097 101 ////////////////////////////////////////////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 102
lpisuseful 2:9b9db9c1c097 103 uIPEthernet.begin(MY_MAC, MY_IP);
lpisuseful 2:9b9db9c1c097 104
lpisuseful 2:9b9db9c1c097 105 IPAddress localIP = uIPEthernet.localIP();
lpisuseful 2:9b9db9c1c097 106 pc.printf("Local IP = %s\r\n", localIP.toString());
lpisuseful 2:9b9db9c1c097 107 pc.printf("Initialization ");
lpisuseful 2:9b9db9c1c097 108
lpisuseful 2:9b9db9c1c097 109 //馬達初始化
lpisuseful 2:9b9db9c1c097 110 printf("4 Phase Stepper Motor v0.1 - Set zero Program\r\n");
lpisuseful 2:9b9db9c1c097 111 printf("\n\r");
lpisuseful 2:9b9db9c1c097 112 //motor.setzerostep(SetZeroStep,0,step_speed); // number of steps, direction, speed
lpisuseful 2:9b9db9c1c097 113 //printf("Already Set to Zero\n\r");
lpisuseful 2:9b9db9c1c097 114
lpisuseful 2:9b9db9c1c097 115 printf("Enter a number:0~+-180 degree\r\n");
lpisuseful 2:9b9db9c1c097 116 if (udp.begin(MY_PORT))
lpisuseful 2:9b9db9c1c097 117 pc.printf("succeeded.\r\n");
lpisuseful 2:9b9db9c1c097 118 else
lpisuseful 2:9b9db9c1c097 119 pc.printf("failed.\r\n");
lpisuseful 2:9b9db9c1c097 120
lpisuseful 2:9b9db9c1c097 121 while (1)
lpisuseful 2:9b9db9c1c097 122 {
lpisuseful 2:9b9db9c1c097 123 int success;
lpisuseful 2:9b9db9c1c097 124 int size = udp.parsePacket(); //Returns the size of the packet in bytes
lpisuseful 2:9b9db9c1c097 125
lpisuseful 2:9b9db9c1c097 126 if (size > 0)
lpisuseful 2:9b9db9c1c097 127 {
lpisuseful 2:9b9db9c1c097 128 ////////////////////////////////////////////////////////將收的字串轉成整數///////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 129 do
lpisuseful 2:9b9db9c1c097 130 {
lpisuseful 2:9b9db9c1c097 131 char* msg = (char*)malloc(size + 1); //malloc() 運算子會配置一個 int 需要的空間，並傳回該空間的位址，所以使用指標 ptr 來儲存這個位址
lpisuseful 2:9b9db9c1c097 132 int len = udp.read(msg, size + 1);
lpisuseful 2:9b9db9c1c097 133 msg[len] = 0;//??不懂
lpisuseful 2:9b9db9c1c097 134
lpisuseful 2:9b9db9c1c097 135 //mesg=msg;
lpisuseful 2:9b9db9c1c097 136 b = atoi((char*)msg); //字串轉數字
lpisuseful 2:9b9db9c1c097 137 pc.printf("Integer number: %d\r\n",b);
lpisuseful 2:9b9db9c1c097 138
lpisuseful 2:9b9db9c1c097 139 //printf("received mesg: '%s", mesg);
lpisuseful 2:9b9db9c1c097 140 printf("received: '%s", msg);
lpisuseful 2:9b9db9c1c097 141
lpisuseful 2:9b9db9c1c097 142 free(msg);
lpisuseful 2:9b9db9c1c097 143 } while ((size = udp.available()) > 0);
lpisuseful 2:9b9db9c1c097 144
lpisuseful 2:9b9db9c1c097 145 //finish reading this packet:
lpisuseful 2:9b9db9c1c097 146 udp.flush();
lpisuseful 2:9b9db9c1c097 147 printf("'\r\n");
lpisuseful 2:9b9db9c1c097 148
lpisuseful 2:9b9db9c1c097 149
lpisuseful 2:9b9db9c1c097 150
lpisuseful 2:9b9db9c1c097 151 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 152
lpisuseful 2:9b9db9c1c097 153
lpisuseful 2:9b9db9c1c097 154 do
lpisuseful 2:9b9db9c1c097 155 {
lpisuseful 2:9b9db9c1c097 156 //send new packet back to ip/port of client. This also
lpisuseful 2:9b9db9c1c097 157 //configures the current connection to ignore packets from
lpisuseful 2:9b9db9c1c097 158 //other clients!
lpisuseful 2:9b9db9c1c097 159 success = udp.beginPacket(udp.remoteIP(), udp.remotePort());
lpisuseful 2:9b9db9c1c097 160 if (success)
lpisuseful 2:9b9db9c1c097 161 pc.printf("beginPacket: succeeded%\r\n");
lpisuseful 2:9b9db9c1c097 162 else
lpisuseful 2:9b9db9c1c097 163 pc.printf("beginPacket: failed%\r\n");
lpisuseful 2:9b9db9c1c097 164
lpisuseful 2:9b9db9c1c097 165 //beginPacket fails if remote ethaddr is unknown. In this case an
lpisuseful 2:9b9db9c1c097 166 //arp-request is send out first and beginPacket succeeds as soon
lpisuseful 2:9b9db9c1c097 167 //the arp-response is received.
lpisuseful 2:9b9db9c1c097 168 } while (!success);
lpisuseful 2:9b9db9c1c097 169
lpisuseful 2:9b9db9c1c097 170 success = udp.write((uint8_t*)message, strlen(message));//response
lpisuseful 2:9b9db9c1c097 171
lpisuseful 2:9b9db9c1c097 172 if (success)
lpisuseful 2:9b9db9c1c097 173 pc.printf("bytes written: %d\r\n", success);
lpisuseful 2:9b9db9c1c097 174
lpisuseful 2:9b9db9c1c097 175 success = udp.endPacket();
lpisuseful 2:9b9db9c1c097 176
lpisuseful 2:9b9db9c1c097 177 if (success)
lpisuseful 2:9b9db9c1c097 178 pc.printf("endPacket: succeeded%\r\n");
lpisuseful 2:9b9db9c1c097 179 else
lpisuseful 2:9b9db9c1c097 180 pc.printf("endPacket: failed%\r\n");
lpisuseful 2:9b9db9c1c097 181
lpisuseful 2:9b9db9c1c097 182 udp.stop();
lpisuseful 2:9b9db9c1c097 183
lpisuseful 2:9b9db9c1c097 184 //restart with new connection to receive packets from other clients
lpisuseful 2:9b9db9c1c097 185 if (udp.begin(MY_PORT))
lpisuseful 2:9b9db9c1c097 186 pc.printf("restart connection: succeeded%\r\n");
lpisuseful 2:9b9db9c1c097 187 else
lpisuseful 2:9b9db9c1c097 188 pc.printf("restart connection: failed%\r\n");
lpisuseful 2:9b9db9c1c097 189 //馬達開始
lpisuseful 2:9b9db9c1c097 190 if(b>180|b<-180)
lpisuseful 2:9b9db9c1c097 191 {
lpisuseful 2:9b9db9c1c097 192 printf("Enter a number:0~+-180 degree\r\n");
lpisuseful 2:9b9db9c1c097 193 }
lpisuseful 2:9b9db9c1c097 194 else if (b>=-180 & b<=180)
lpisuseful 2:9b9db9c1c097 195 {
lpisuseful 2:9b9db9c1c097 196 if(b!=memory)
lpisuseful 2:9b9db9c1c097 197 {
lpisuseful 2:9b9db9c1c097 198
lpisuseful 2:9b9db9c1c097 199 printf("Now b: %d\n\r", b);
lpisuseful 2:9b9db9c1c097 200 scan_int(b);
lpisuseful 2:9b9db9c1c097 201 c=trans(vardegree);
lpisuseful 2:9b9db9c1c097 202
lpisuseful 2:9b9db9c1c097 203 if (b > memorydegree)
lpisuseful 2:9b9db9c1c097 204 {
lpisuseful 2:9b9db9c1c097 205 motor.step(c,0,step_speed); // dir=0; 順時鐘
lpisuseful 2:9b9db9c1c097 206 //printf("Current b: %d\n\r", b);
lpisuseful 2:9b9db9c1c097 207 }
lpisuseful 2:9b9db9c1c097 208 if (b < memorydegree)
lpisuseful 2:9b9db9c1c097 209 {
lpisuseful 2:9b9db9c1c097 210 motor.step(c,1,step_speed);//dir=1;逆時鐘
lpisuseful 2:9b9db9c1c097 211 //printf("Current b: %d\n\r", b);
lpisuseful 2:9b9db9c1c097 212 }
lpisuseful 2:9b9db9c1c097 213 printf("Enter a number:0~+-180 degree\r\n");
lpisuseful 2:9b9db9c1c097 214
lpisuseful 2:9b9db9c1c097 215 memorydegree=b;
lpisuseful 2:9b9db9c1c097 216 //printf("memorydegree: %d\n\r",memorydegree);
lpisuseful 2:9b9db9c1c097 217 memory=b;
lpisuseful 2:9b9db9c1c097 218 }
lpisuseful 2:9b9db9c1c097 219 //wait(1);
lpisuseful 2:9b9db9c1c097 220 }
lpisuseful 2:9b9db9c1c097 221 }
lpisuseful 2:9b9db9c1c097 222 }
lpisuseful 2:9b9db9c1c097 223
lpisuseful 2:9b9db9c1c097 224
lpisuseful 2:9b9db9c1c097 225
lpisuseful 2:9b9db9c1c097 226
lpisuseful 2:9b9db9c1c097 227
lpisuseful 2:9b9db9c1c097 228
lpisuseful 2:9b9db9c1c097 229 ///////////////////////////////////////////////////////////////////////////////////////////////
lpisuseful 2:9b9db9c1c097 230
lpisuseful 2:9b9db9c1c097 231
lpisuseful 2:9b9db9c1c097 232
lpisuseful 2:9b9db9c1c097 233
lpisuseful 2:9b9db9c1c097 234
lpisuseful 2:9b9db9c1c097 235
lpisuseful 2:9b9db9c1c097 236
lpisuseful 2:9b9db9c1c097 237
lpisuseful 2:9b9db9c1c097 238
lpisuseful 2:9b9db9c1c097 239
lpisuseful 2:9b9db9c1c097 240
lpisuseful 2:9b9db9c1c097 241 printf("Default Speed: %d\n\r",step_speed);
lpisuseful 2:9b9db9c1c097 242 printf("1- 360 degree clockwise step\n\r");
lpisuseful 2:9b9db9c1c097 243 printf("2- 360 degree anticlockwise step\n\r");
lpisuseful 2:9b9db9c1c097 244 printf("3- 180 degree clockwise step\n\r");
lpisuseful 2:9b9db9c1c097 245 printf("4- 180 degree anticlockwise step\n\r");
lpisuseful 2:9b9db9c1c097 246 printf("5- Change Speed\n\r");
lpisuseful 2:9b9db9c1c097 247
lpisuseful 2:9b9db9c1c097 248
lpisuseful 2:9b9db9c1c097 249
lpisuseful 2:9b9db9c1c097 250 while(1)
lpisuseful 2:9b9db9c1c097 251 {
lpisuseful 2:9b9db9c1c097 252 if (pc.readable())
lpisuseful 2:9b9db9c1c097 253 { // checks for serial
lpisuseful 2:9b9db9c1c097 254
lpisuseful 2:9b9db9c1c097 255 if (pc.getc()=='1')
lpisuseful 2:9b9db9c1c097 256 motor.step(numstep,0,step_speed); // number of steps, direction, speed
lpisuseful 2:9b9db9c1c097 257
lpisuseful 2:9b9db9c1c097 258 if (pc.getc()=='2')
lpisuseful 2:9b9db9c1c097 259 motor.step(numstep,1,step_speed);
lpisuseful 2:9b9db9c1c097 260
lpisuseful 2:9b9db9c1c097 261 if (pc.getc()=='3')
lpisuseful 2:9b9db9c1c097 262 motor.step(numstep/2,0,step_speed);
lpisuseful 2:9b9db9c1c097 263
lpisuseful 2:9b9db9c1c097 264 if (pc.getc()=='4')
lpisuseful 2:9b9db9c1c097 265 motor.step(numstep/2,1,step_speed);
lpisuseful 2:9b9db9c1c097 266
lpisuseful 2:9b9db9c1c097 267 if (pc.getc()=='5')
lpisuseful 2:9b9db9c1c097 268 {
lpisuseful 2:9b9db9c1c097 269 printf("Current Speed: %d\n\r", step_speed);
lpisuseful 2:9b9db9c1c097 270 printf("New speed: \n\r");
lpisuseful 2:9b9db9c1c097 271 pc.scanf("%d",&step_speed); // sets new speed
lpisuseful 2:9b9db9c1c097 272 }
lpisuseful 2:9b9db9c1c097 273 }
lpisuseful 2:9b9db9c1c097 274 }
lpisuseful 2:9b9db9c1c097 275
lpisuseful 2:9b9db9c1c097 276
lpisuseful 2:9b9db9c1c097 277 }
lpisuseful 2:9b9db9c1c097 278
lpisuseful 2:9b9db9c1c097 279
lpisuseful 2:9b9db9c1c097 280
lpisuseful 2:9b9db9c1c097 281
lpisuseful 2:9b9db9c1c097 282
lpisuseful 2:9b9db9c1c097 283 void sMotor::setzerostep(int SetZeroSteps, int direction, int step_speed)
lpisuseful 2:9b9db9c1c097 284 {// steper function: number of steps, direction (0- right, 1- left), speed (default 1200)
lpisuseful 2:9b9db9c1c097 285 //printf("test\r\n");
lpisuseful 2:9b9db9c1c097 286 int count=0; // initalize step count
lpisuseful 2:9b9db9c1c097 287 if (direction==0) // turn clockwise
lpisuseful 2:9b9db9c1c097 288 {
lpisuseful 2:9b9db9c1c097 289 printf("test");
lpisuseful 2:9b9db9c1c097 290 do
lpisuseful 2:9b9db9c1c097 291 {
lpisuseful 2:9b9db9c1c097 292 SetZeroClockWise();
lpisuseful 2:9b9db9c1c097 293 count++;
lpisuseful 2:9b9db9c1c097 294 }while (count<numstep & set_zero_point==0); // turn number of steps applied
lpisuseful 2:9b9db9c1c097 295 if (set_zero_point==1) // 如果光遮斷器訊號被中斷則LED亮起。
lpisuseful 2:9b9db9c1c097 296 {
lpisuseful 2:9b9db9c1c097 297 LED=0;
lpisuseful 2:9b9db9c1c097 298 printf("Already Set to Zero\n\r");
lpisuseful 2:9b9db9c1c097 299 }
lpisuseful 2:9b9db9c1c097 300 //printf("test\r\n");
lpisuseful 2:9b9db9c1c097 301
lpisuseful 2:9b9db9c1c097 302
lpisuseful 2:9b9db9c1c097 303
lpisuseful 2:9b9db9c1c097 304 }
lpisuseful 2:9b9db9c1c097 305 zerocount++;
lpisuseful 2:9b9db9c1c097 306 }
lpisuseful 2:9b9db9c1c097 307
lpisuseful 2:9b9db9c1c097 308
lpisuseful 2:9b9db9c1c097 309
lpisuseful 2:9b9db9c1c097 310
lpisuseful 2:9b9db9c1c097 311
lpisuseful 2:9b9db9c1c097 312
lpisuseful 2:9b9db9c1c097 313
lpisuseful 2:9b9db9c1c097 314
lpisuseful 2:9b9db9c1c097 315 void sMotor::SetZeroClockWise()
lpisuseful 2:9b9db9c1c097 316 {
lpisuseful 2:9b9db9c1c097 317
lpisuseful 2:9b9db9c1c097 318 //set_zero_step = setzeroclockwise;
lpisuseful 2:9b9db9c1c097 319 for(int i = 0 ; i < 8 ; i++)
lpisuseful 2:9b9db9c1c097 320 {
lpisuseful 2:9b9db9c1c097 321 //i = set_zero_step%8;
lpisuseful 2:9b9db9c1c097 322 switch (i)
lpisuseful 2:9b9db9c1c097 323 {
lpisuseful 2:9b9db9c1c097 324 case 0:
lpisuseful 2:9b9db9c1c097 325 {
lpisuseful 2:9b9db9c1c097 326 _A0=1;
lpisuseful 2:9b9db9c1c097 327 _A1=0;
lpisuseful 2:9b9db9c1c097 328 _A2=0;
lpisuseful 2:9b9db9c1c097 329 _A3=1;
lpisuseful 2:9b9db9c1c097 330 }
lpisuseful 2:9b9db9c1c097 331 break;
lpisuseful 2:9b9db9c1c097 332 case 1:
lpisuseful 2:9b9db9c1c097 333 {
lpisuseful 2:9b9db9c1c097 334 _A0=1;
lpisuseful 2:9b9db9c1c097 335 _A1=0;
lpisuseful 2:9b9db9c1c097 336 _A2=0;
lpisuseful 2:9b9db9c1c097 337 _A3=0;
lpisuseful 2:9b9db9c1c097 338 }
lpisuseful 2:9b9db9c1c097 339 break;
lpisuseful 2:9b9db9c1c097 340 case 2:
lpisuseful 2:9b9db9c1c097 341 {
lpisuseful 2:9b9db9c1c097 342 _A0=1;
lpisuseful 2:9b9db9c1c097 343 _A1=1;
lpisuseful 2:9b9db9c1c097 344 _A2=0;
lpisuseful 2:9b9db9c1c097 345 _A3=0;
lpisuseful 2:9b9db9c1c097 346 }
lpisuseful 2:9b9db9c1c097 347 break;
lpisuseful 2:9b9db9c1c097 348 case 3:
lpisuseful 2:9b9db9c1c097 349 {
lpisuseful 2:9b9db9c1c097 350 _A0=0;
lpisuseful 2:9b9db9c1c097 351 _A1=1;
lpisuseful 2:9b9db9c1c097 352 _A2=0;
lpisuseful 2:9b9db9c1c097 353 _A3=0;
lpisuseful 2:9b9db9c1c097 354 }
lpisuseful 2:9b9db9c1c097 355 break;
lpisuseful 2:9b9db9c1c097 356 case 4:
lpisuseful 2:9b9db9c1c097 357 {
lpisuseful 2:9b9db9c1c097 358 _A0=0;
lpisuseful 2:9b9db9c1c097 359 _A1=1;
lpisuseful 2:9b9db9c1c097 360 _A2=1;
lpisuseful 2:9b9db9c1c097 361 _A3=0;
lpisuseful 2:9b9db9c1c097 362 }
lpisuseful 2:9b9db9c1c097 363 break;
lpisuseful 2:9b9db9c1c097 364 case 5:
lpisuseful 2:9b9db9c1c097 365 {
lpisuseful 2:9b9db9c1c097 366 _A0=0;
lpisuseful 2:9b9db9c1c097 367 _A1=0;
lpisuseful 2:9b9db9c1c097 368 _A2=1;
lpisuseful 2:9b9db9c1c097 369 _A3=0;
lpisuseful 2:9b9db9c1c097 370 }
lpisuseful 2:9b9db9c1c097 371 break;
lpisuseful 2:9b9db9c1c097 372 case 6:
lpisuseful 2:9b9db9c1c097 373 {
lpisuseful 2:9b9db9c1c097 374 _A0=0;
lpisuseful 2:9b9db9c1c097 375 _A1=0;
lpisuseful 2:9b9db9c1c097 376 _A2=1;
lpisuseful 2:9b9db9c1c097 377 _A3=1;
lpisuseful 2:9b9db9c1c097 378 }
lpisuseful 2:9b9db9c1c097 379 break;
lpisuseful 2:9b9db9c1c097 380 case 7:
lpisuseful 2:9b9db9c1c097 381 {
lpisuseful 2:9b9db9c1c097 382 _A0=0;
lpisuseful 2:9b9db9c1c097 383 _A1=0;
lpisuseful 2:9b9db9c1c097 384 _A2=0;
lpisuseful 2:9b9db9c1c097 385 _A3=1;
lpisuseful 2:9b9db9c1c097 386 }
lpisuseful 2:9b9db9c1c097 387 break;
lpisuseful 2:9b9db9c1c097 388 }//switch
lpisuseful 2:9b9db9c1c097 389 wait_us(step_speed); // wait time defines the speed
lpisuseful 2:9b9db9c1c097 390 }//for
lpisuseful 2:9b9db9c1c097 391
lpisuseful 2:9b9db9c1c097 392 }
lpisuseful 2:9b9db9c1c097 393
lpisuseful 2:9b9db9c1c097 394
lpisuseful 2:9b9db9c1c097 395
lpisuseful 2:9b9db9c1c097 396
lpisuseful 2:9b9db9c1c097 397
lpisuseful 2:9b9db9c1c097 398
lpisuseful 2:9b9db9c1c097 399
lpisuseful 2:9b9db9c1c097 400
lpisuseful 2:9b9db9c1c097 401 void scan_int(int scan)//變化量函數
lpisuseful 2:9b9db9c1c097 402 {
lpisuseful 2:9b9db9c1c097 403 //scanf("%d",&b);
lpisuseful 2:9b9db9c1c097 404 //printf("Enter b: %d\n\r",b);
lpisuseful 2:9b9db9c1c097 405
lpisuseful 2:9b9db9c1c097 406 vardegree=b-memorydegree;//變化量=輸入度數-上次度數
lpisuseful 2:9b9db9c1c097 407 printf("vardegree: %d\n\r",vardegree);
lpisuseful 2:9b9db9c1c097 408 fflush(stdout);
lpisuseful 2:9b9db9c1c097 409
lpisuseful 2:9b9db9c1c097 410 }
lpisuseful 2:9b9db9c1c097 411 int trans(int trans)//度數轉步數
lpisuseful 2:9b9db9c1c097 412 {
lpisuseful 2:9b9db9c1c097 413 float numstep = 0 ; // defines full turn of 360 degree
lpisuseful 2:9b9db9c1c097 414 int d=0;
lpisuseful 2:9b9db9c1c097 415 numstep=1.422222*trans;//取實際步數值
lpisuseful 2:9b9db9c1c097 416 if(numstep<0)
lpisuseful 2:9b9db9c1c097 417 {
lpisuseful 2:9b9db9c1c097 418 numstep=-numstep;
lpisuseful 2:9b9db9c1c097 419 }
lpisuseful 2:9b9db9c1c097 420 d=int(numstep+0.5);//四捨五入整數步數
lpisuseful 2:9b9db9c1c097 421 //printf("Real steps d: %d\n\r",d);
lpisuseful 2:9b9db9c1c097 422 return d;
lpisuseful 2:9b9db9c1c097 423 }