Diff: main.cpp

Revision:

0:fc3d2482b2f7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 20 21:34:45 2018 +0000
@@ -0,0 +1,139 @@
+#include "mbed.h"
+#include "TSISensor.h"
+#define ADE 0.7
+#define ATR 0.3
+
+
+DigitalOut LR(LED1);
+DigitalOut LG(LED2);
+DigitalOut LA(LED3);
+PwmOut O1 (PTB0);
+PwmOut O2 (PTB1);
+AnalogIn AnalogicoXY(PTB2);
+Ticker servo1;
+
+
+char DIRE=0;
+char HABO1=0;
+char HABO2=0;
+
+
+void servo1f (void);
+
+void ME_servocontrol(void);
+
+enum {PM,LEV_PATA,ADE_PATA,BAJAR_PATA,ATR_PATA};
+
+char ME_servocontrol_estado=0;
+
+TSISensor tsi;
+
+int main()
+{
+
+    servo1.attach(&servo1f,0.05);
+    //  servo1.attach(&ME_servocontrol,1);
+//    myled.period(200);
+//    myled=0.05;
+    O1=0.025;
+    O2=0.025;
+    while(1) {
+        ME_servocontrol();
+    }
+}
+
+void servo1f ()
+{
+    if(HABO1==1 && DIRE==1)
+        O1=O1+(0.005);
+    if(HABO2==1 && DIRE==1)
+        O2=O2+(0.005);
+    if(HABO1==1 && DIRE==0)
+        O1=O1-(0.005);
+    if(HABO2==1 && DIRE==0)
+        O2=O2-(0.005);
+}
+
+void ME_servocontrol()
+{
+    switch(ME_servocontrol_estado) {
+        case PM:
+            O1=0.04625;
+            O2=0.0025;
+            HABO1=0;
+            HABO2=0;
+            LR=0;
+            LG=1;
+            LA=1;
+            if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 || /*AnalogicoXY<ATR*/ tsi.readPercentage()<0.35 && tsi.readPercentage()>0) ME_servocontrol_estado=LEV_PATA;
+            break;
+        case LEV_PATA:
+            HABO1=0;
+            HABO2=1;
+            DIRE=1;
+            LR=1;
+            LG=0;
+            LA=1;
+            if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 && O2>=0.0625) {
+                ME_servocontrol_estado=ADE_PATA;
+            }
+            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2>=0.0625) {
+                ME_servocontrol_estado=ATR_PATA;
+            }
+            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
+                ME_servocontrol_estado=PM;
+            }
+            break;
+        case ADE_PATA:
+            HABO1=1;
+            HABO2=0;
+            DIRE=1;
+            LR=1;
+            LG=1;
+            LA=0;
+            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1>=0.0625) {
+                ME_servocontrol_estado=BAJAR_PATA;
+            }
+            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1>=0.0625) {
+                ME_servocontrol_estado=LEV_PATA;
+            }
+            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
+                ME_servocontrol_estado=PM;
+            }
+            break;
+        case BAJAR_PATA:
+            HABO1=0;
+            HABO2=1;
+            DIRE=0;
+            LR=1;
+            LG=0;
+            LA=0;
+            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O2<=0.025) {
+                ME_servocontrol_estado=ATR_PATA;
+            }
+            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2<=0.025) {
+                ME_servocontrol_estado=ADE_PATA;
+            }
+            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
+                ME_servocontrol_estado=PM;
+            }
+            break;
+        case ATR_PATA:
+            HABO1=1;
+            HABO2=0;
+            DIRE=0;
+            LR=0;
+            LG=1;
+            LA=0;
+            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1<=0.025) {
+                ME_servocontrol_estado=LEV_PATA;
+            }
+            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1<=0.025) {
+                ME_servocontrol_estado=BAJAR_PATA;
+            }
+            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
+                ME_servocontrol_estado=PM;
+            }
+            break;
+    }
+}
\ No newline at end of file