Diff: main.cpp

Revision:

0:0705184c2e58

diff -r 000000000000 -r 0705184c2e58 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 23 07:36:32 2019 +0000
@@ -0,0 +1,85 @@
+//Program untuk Arm dengan PID tetha
+#include "mbed.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+
+encoderKRAI encoderPelontar (PC_15, PC_14, 538, encoderKRAI::X4_ENCODING);
+Motor motorPelontar(PB_1, PB_2, PB_15);
+DigitalIn butt (USER_BUTTON);
+DigitalOut pneu[3] = {(PC_7), (PC_8), (PC_9)}; 
+//pneu 0, 1 = pneu buat ambil shagai
+//pneu 2 = pneu buat lempar shagai
+
+Serial pc(USBTX, USBRX,115200);
+double theta;
+double pulse;
+int kondisi=0, cp=0;
+uint32_t millisx;
+ 
+
+void hitungParameter();
+
+int main(){
+   
+    theta=0.0;
+    encoderPelontar.reset();
+ 
+      pneu[0] = 1;
+      pneu[1] = 1;
+      pneu[2] = 0;
+      
+while (1){
+    // 2, 4, 6 mati ketika 1 namun led nyala
+    //0 tuh narik kedalem
+
+   /* if (butt){
+      pneu[0] = 0; //led 2
+      pneu[1] = 1; //4
+      pneu[2] = 1; //6
+    }
+      else {
+      pneu[0] = 1;
+      pneu[1] = 1;
+      pneu[2] = 1;
+          }
+
+*/
+
+if (!butt){
+    cp = 1;
+    wait(1);
+} 
+    
+if(cp){
+    pneu[0] = 1;
+    pneu[2] = 0;
+    while (theta<160.0){ 
+        motorPelontar.speed(0.7);
+        hitungParameter();
+        }
+    motorPelontar.brake(BRAKE_HIGH);
+    wait (1);
+    pneu[0] = 0;
+    wait_ms (1000);
+    
+    while (theta>30.0){
+        motorPelontar.speed(-0.2);
+        hitungParameter();
+        }
+    motorPelontar.brake(BRAKE_HIGH);
+    wait (1);
+    pneu[2] = 1;
+    wait (1);
+    pneu[2] = 0;
+    cp=0;
+}
+}
+}
+
+void hitungParameter(){
+    pulse=(double) encoderPelontar.getPulses()*360/538;
+    encoderPelontar.reset();
+    theta+=pulse;
+    pc.printf("theta = %.2f\t\n", theta );
+}
\ No newline at end of file