KRTMI 2019
/
Inverse_Kine_Lengan3New
Peningkatan Ketelitian
Diff: main.cpp
- Revision:
- 0:9dfae2e18414
- Child:
- 1:9284c57e84da
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Apr 10 15:18:37 2019 +0000 @@ -0,0 +1,66 @@ +#include "mbed.h" +#include "Servo.h" +#include "Motor.h" + +/* Nama servo + 4 + 3 + 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); + +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); +} + +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); + + //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) +}