yi li
/
3W_wifi_odo_p2p_0722
20160725
Diff: main.cpp
- Revision:
- 1:a78e5f6154d3
- Parent:
- 0:926cd923a480
- Child:
- 2:388988e5733a
diff -r 926cd923a480 -r a78e5f6154d3 main.cpp --- a/main.cpp Fri Jul 22 07:57:56 2016 +0000 +++ b/main.cpp Mon Jul 25 06:38:27 2016 +0000 @@ -66,8 +66,17 @@ float err[3]= {0}; bool s0 = false; char dire; +char c_for_get; +char c_for_send; +SPI spi(D11, D12, D13); // mosi, miso, sclk +DigitalOut cs(D10); +Serial pc(SERIAL_TX, SERIAL_RX); +char *delim = " "; +char *pch; +float cha_L[3]; + int main() { @@ -144,7 +153,7 @@ XL[2]=0+pi/3; break; } - + printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100); printf("Initial Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100 ,Current_X[2]*180/pi-60); printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60); @@ -159,6 +168,32 @@ } while(1) { + + cs = 0; + for (const char *p = "32\n" ; c_for_get = *p; p++) + { + spi.write (c_for_get); + } + wait(0.1); + float bb = spi.write (0x00); + char ddest[3]; + sprintf(ddest,"%.1f \n", bb); + pc.printf(ddest); + cs = 1;//須將ddest矩陣拆開成三個不同的數 + + int strtok_n = 0; + pch = strtok(ddest,delim); + while(pch != NULL) + { + cha_L[strtok_n] = atof(pch); + //printf("%s\n",pch); + pch = strtok(NULL, delim); + strtok_n = strtok_n+1; + } + XL[0] = cha_L[0]; + XL[1] = cha_L[1]; + XL[2] = cha_L[2]; + // myled = 0; X1[0] = Current_X[0]; @@ -233,6 +268,17 @@ err[1] = Current_X[1]-XL[1]; err[2] = Current_X[2]-XL[2]; // END // + char send_array[6]; + sprintf(send_array, "%.1f %.1f %.1f\n", Current_X[0], Current_X[1], Current_X[2]); + + cs = 0; + for (const char *p = send_array ; c = *p; p++) //尚未實測 + { + spi.write (c); + } + wait(0.1); + cs = 1; + if ( t.read_ms() > 200) { // print t.reset(); @@ -280,6 +326,6 @@ */ } - wait_ms(10);// 會有什麼影響? + wait_ms(50);// 會有什麼影響? } }