KRTMI 2019
/
Inverse_Kine_Lengan3New
Peningkatan Ketelitian
Diff: main.cpp
- Revision:
- 1:9284c57e84da
- Parent:
- 0:9dfae2e18414
- Child:
- 2:34ed8b55eba6
--- a/main.cpp Wed Apr 10 15:18:37 2019 +0000 +++ b/main.cpp Sat Apr 13 18:51:43 2019 +0000 @@ -8,59 +8,37 @@ 21 22 1 */ -Servo servo1(PA_1); -Servo servo21(PA_2); -Servo servo22(PA_3); -Servo servo3(PA_4); -Servo servo4(PA_5); -Motor motorKanan(PB_3, PA_10, PC_4); -Motor motorKiri(PB_7, PC_3, PC_0); +Servo servo21(PA_7); +Servo servo22(PB_6); +Serial pc(D1, D0); -void kalibrasi(float deg){ - servo1.calibrate(0.0005,deg); - servo21.calibrate(0.0005,deg); - servo22.calibrate(0.0005,deg); - servo3.calibrate(0.0005,deg); - servo4.calibrate(0.0005,deg); -} - -void idle(float s1,float s21,float s22, float s3, float s4){ - servo1.position(s1); - wait_ms(1); - servo21.position(s21); - servo22.position(s22); - wait_ms(1); - servo3.position(s3); - wait_ms(1); - servo4.position(s4); - wait_ms(1); +void servo2(float f, float g){ + servo21.write(f); + servo22.write(g); + pc.printf("%f", servo21.read()); + pc.printf("\n"); + pc.printf("%f", servo22.read()); + pc.printf("\n"); } int main() { - /***Gerak Jalan*** - //Maju - motorKanan.speed(0.3); - motorKiri.speed(0.3); - - //Mundur - motorKanan.speed(0.3); - motorKiri.speed(0.3); - - //PivotKiri - motorKanan.brake(); - motorKiri.speed(0.3); + pc.baud(9600); + pc.printf("test begin\n"); - //PivotKanan - motorKiri.brake(); - motorKanan.speed(0.3); - */ - - /***Gerak Tangan***/ - //kalibrasi - kalibrasi(300); - - //mode1(idle) - idle(0.0,90.0,90.0,90.0,0.0); - - //mode2(gerakan keluar) + //tes + servo21.calibrate(0.001,150); + servo22.calibrate(0.001,150); + servo21.write(0); + servo22.write(1.0); + while(1){ + float m=0.0; + float n=0.0; + pc.scanf("%f",&m); + pc.printf("test \n"); + pc.scanf("%f",&n); + pc.printf("test \n"); + float f = m; + float g = n; + servo2(f,g); + } }