ver 1 yang belum behrasil, motor belum gerak kalo kodingannya digabung
Dependencies: mbed encoderKRAI Motornew millis
main.cpp@0:0705184c2e58, 2019-02-23 (annotated)
- Committer:
- 315_josh
- Date:
- Sat Feb 23 07:36:32 2019 +0000
- Revision:
- 0:0705184c2e58
versi 1 yang masih belum berhasil
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
315_josh | 0:0705184c2e58 | 1 | //Program untuk Arm dengan PID tetha |
315_josh | 0:0705184c2e58 | 2 | #include "mbed.h" |
315_josh | 0:0705184c2e58 | 3 | #include "Motor.h" |
315_josh | 0:0705184c2e58 | 4 | #include "encoderKRAI.h" |
315_josh | 0:0705184c2e58 | 5 | #include "millis.h" |
315_josh | 0:0705184c2e58 | 6 | |
315_josh | 0:0705184c2e58 | 7 | encoderKRAI encoderPelontar (PC_15, PC_14, 538, encoderKRAI::X4_ENCODING); |
315_josh | 0:0705184c2e58 | 8 | Motor motorPelontar(PB_1, PB_2, PB_15); |
315_josh | 0:0705184c2e58 | 9 | DigitalIn butt (USER_BUTTON); |
315_josh | 0:0705184c2e58 | 10 | DigitalOut pneu[3] = {(PC_7), (PC_8), (PC_9)}; |
315_josh | 0:0705184c2e58 | 11 | //pneu 0, 1 = pneu buat ambil shagai |
315_josh | 0:0705184c2e58 | 12 | //pneu 2 = pneu buat lempar shagai |
315_josh | 0:0705184c2e58 | 13 | |
315_josh | 0:0705184c2e58 | 14 | Serial pc(USBTX, USBRX,115200); |
315_josh | 0:0705184c2e58 | 15 | double theta; |
315_josh | 0:0705184c2e58 | 16 | double pulse; |
315_josh | 0:0705184c2e58 | 17 | int kondisi=0, cp=0; |
315_josh | 0:0705184c2e58 | 18 | uint32_t millisx; |
315_josh | 0:0705184c2e58 | 19 | |
315_josh | 0:0705184c2e58 | 20 | |
315_josh | 0:0705184c2e58 | 21 | void hitungParameter(); |
315_josh | 0:0705184c2e58 | 22 | |
315_josh | 0:0705184c2e58 | 23 | int main(){ |
315_josh | 0:0705184c2e58 | 24 | |
315_josh | 0:0705184c2e58 | 25 | theta=0.0; |
315_josh | 0:0705184c2e58 | 26 | encoderPelontar.reset(); |
315_josh | 0:0705184c2e58 | 27 | |
315_josh | 0:0705184c2e58 | 28 | pneu[0] = 1; |
315_josh | 0:0705184c2e58 | 29 | pneu[1] = 1; |
315_josh | 0:0705184c2e58 | 30 | pneu[2] = 0; |
315_josh | 0:0705184c2e58 | 31 | |
315_josh | 0:0705184c2e58 | 32 | while (1){ |
315_josh | 0:0705184c2e58 | 33 | // 2, 4, 6 mati ketika 1 namun led nyala |
315_josh | 0:0705184c2e58 | 34 | //0 tuh narik kedalem |
315_josh | 0:0705184c2e58 | 35 | |
315_josh | 0:0705184c2e58 | 36 | /* if (butt){ |
315_josh | 0:0705184c2e58 | 37 | pneu[0] = 0; //led 2 |
315_josh | 0:0705184c2e58 | 38 | pneu[1] = 1; //4 |
315_josh | 0:0705184c2e58 | 39 | pneu[2] = 1; //6 |
315_josh | 0:0705184c2e58 | 40 | } |
315_josh | 0:0705184c2e58 | 41 | else { |
315_josh | 0:0705184c2e58 | 42 | pneu[0] = 1; |
315_josh | 0:0705184c2e58 | 43 | pneu[1] = 1; |
315_josh | 0:0705184c2e58 | 44 | pneu[2] = 1; |
315_josh | 0:0705184c2e58 | 45 | } |
315_josh | 0:0705184c2e58 | 46 | |
315_josh | 0:0705184c2e58 | 47 | */ |
315_josh | 0:0705184c2e58 | 48 | |
315_josh | 0:0705184c2e58 | 49 | if (!butt){ |
315_josh | 0:0705184c2e58 | 50 | cp = 1; |
315_josh | 0:0705184c2e58 | 51 | wait(1); |
315_josh | 0:0705184c2e58 | 52 | } |
315_josh | 0:0705184c2e58 | 53 | |
315_josh | 0:0705184c2e58 | 54 | if(cp){ |
315_josh | 0:0705184c2e58 | 55 | pneu[0] = 1; |
315_josh | 0:0705184c2e58 | 56 | pneu[2] = 0; |
315_josh | 0:0705184c2e58 | 57 | while (theta<160.0){ |
315_josh | 0:0705184c2e58 | 58 | motorPelontar.speed(0.7); |
315_josh | 0:0705184c2e58 | 59 | hitungParameter(); |
315_josh | 0:0705184c2e58 | 60 | } |
315_josh | 0:0705184c2e58 | 61 | motorPelontar.brake(BRAKE_HIGH); |
315_josh | 0:0705184c2e58 | 62 | wait (1); |
315_josh | 0:0705184c2e58 | 63 | pneu[0] = 0; |
315_josh | 0:0705184c2e58 | 64 | wait_ms (1000); |
315_josh | 0:0705184c2e58 | 65 | |
315_josh | 0:0705184c2e58 | 66 | while (theta>30.0){ |
315_josh | 0:0705184c2e58 | 67 | motorPelontar.speed(-0.2); |
315_josh | 0:0705184c2e58 | 68 | hitungParameter(); |
315_josh | 0:0705184c2e58 | 69 | } |
315_josh | 0:0705184c2e58 | 70 | motorPelontar.brake(BRAKE_HIGH); |
315_josh | 0:0705184c2e58 | 71 | wait (1); |
315_josh | 0:0705184c2e58 | 72 | pneu[2] = 1; |
315_josh | 0:0705184c2e58 | 73 | wait (1); |
315_josh | 0:0705184c2e58 | 74 | pneu[2] = 0; |
315_josh | 0:0705184c2e58 | 75 | cp=0; |
315_josh | 0:0705184c2e58 | 76 | } |
315_josh | 0:0705184c2e58 | 77 | } |
315_josh | 0:0705184c2e58 | 78 | } |
315_josh | 0:0705184c2e58 | 79 | |
315_josh | 0:0705184c2e58 | 80 | void hitungParameter(){ |
315_josh | 0:0705184c2e58 | 81 | pulse=(double) encoderPelontar.getPulses()*360/538; |
315_josh | 0:0705184c2e58 | 82 | encoderPelontar.reset(); |
315_josh | 0:0705184c2e58 | 83 | theta+=pulse; |
315_josh | 0:0705184c2e58 | 84 | pc.printf("theta = %.2f\t\n", theta ); |
315_josh | 0:0705184c2e58 | 85 | } |