Juan Agustin Otero / Mbed 2 deprecated Robot_Leg_Driver

Driver para movimiento de una pierna electronica

Dependencies:   TSI mbed

main.cpp@0:fc3d2482b2f7, 2018-04-20 (annotated)

Committer:
JAgustinOtero
Date:
Fri Apr 20 21:34:45 2018 +0000
Revision:
0:fc3d2482b2f7
Driver para movimiento de una pierna robotica de 2 servos;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JAgustinOtero 0:fc3d2482b2f7 1 #include "mbed.h"
JAgustinOtero 0:fc3d2482b2f7 2 #include "TSISensor.h"
JAgustinOtero 0:fc3d2482b2f7 3 #define ADE 0.7
JAgustinOtero 0:fc3d2482b2f7 4 #define ATR 0.3
JAgustinOtero 0:fc3d2482b2f7 5
JAgustinOtero 0:fc3d2482b2f7 6
JAgustinOtero 0:fc3d2482b2f7 7 DigitalOut LR(LED1);
JAgustinOtero 0:fc3d2482b2f7 8 DigitalOut LG(LED2);
JAgustinOtero 0:fc3d2482b2f7 9 DigitalOut LA(LED3);
JAgustinOtero 0:fc3d2482b2f7 10 PwmOut O1 (PTB0);
JAgustinOtero 0:fc3d2482b2f7 11 PwmOut O2 (PTB1);
JAgustinOtero 0:fc3d2482b2f7 12 AnalogIn AnalogicoXY(PTB2);
JAgustinOtero 0:fc3d2482b2f7 13 Ticker servo1;
JAgustinOtero 0:fc3d2482b2f7 14
JAgustinOtero 0:fc3d2482b2f7 15
JAgustinOtero 0:fc3d2482b2f7 16 char DIRE=0;
JAgustinOtero 0:fc3d2482b2f7 17 char HABO1=0;
JAgustinOtero 0:fc3d2482b2f7 18 char HABO2=0;
JAgustinOtero 0:fc3d2482b2f7 19
JAgustinOtero 0:fc3d2482b2f7 20
JAgustinOtero 0:fc3d2482b2f7 21 void servo1f (void);
JAgustinOtero 0:fc3d2482b2f7 22
JAgustinOtero 0:fc3d2482b2f7 23 void ME_servocontrol(void);
JAgustinOtero 0:fc3d2482b2f7 24
JAgustinOtero 0:fc3d2482b2f7 25 enum {PM,LEV_PATA,ADE_PATA,BAJAR_PATA,ATR_PATA};
JAgustinOtero 0:fc3d2482b2f7 26
JAgustinOtero 0:fc3d2482b2f7 27 char ME_servocontrol_estado=0;
JAgustinOtero 0:fc3d2482b2f7 28
JAgustinOtero 0:fc3d2482b2f7 29 TSISensor tsi;
JAgustinOtero 0:fc3d2482b2f7 30
JAgustinOtero 0:fc3d2482b2f7 31 int main()
JAgustinOtero 0:fc3d2482b2f7 32 {
JAgustinOtero 0:fc3d2482b2f7 33
JAgustinOtero 0:fc3d2482b2f7 34 servo1.attach(&servo1f,0.05);
JAgustinOtero 0:fc3d2482b2f7 35 // servo1.attach(&ME_servocontrol,1);
JAgustinOtero 0:fc3d2482b2f7 36 // myled.period(200);
JAgustinOtero 0:fc3d2482b2f7 37 // myled=0.05;
JAgustinOtero 0:fc3d2482b2f7 38 O1=0.025;
JAgustinOtero 0:fc3d2482b2f7 39 O2=0.025;
JAgustinOtero 0:fc3d2482b2f7 40 while(1) {
JAgustinOtero 0:fc3d2482b2f7 41 ME_servocontrol();
JAgustinOtero 0:fc3d2482b2f7 42 }
JAgustinOtero 0:fc3d2482b2f7 43 }
JAgustinOtero 0:fc3d2482b2f7 44
JAgustinOtero 0:fc3d2482b2f7 45 void servo1f ()
JAgustinOtero 0:fc3d2482b2f7 46 {
JAgustinOtero 0:fc3d2482b2f7 47 if(HABO1==1 && DIRE==1)
JAgustinOtero 0:fc3d2482b2f7 48 O1=O1+(0.005);
JAgustinOtero 0:fc3d2482b2f7 49 if(HABO2==1 && DIRE==1)
JAgustinOtero 0:fc3d2482b2f7 50 O2=O2+(0.005);
JAgustinOtero 0:fc3d2482b2f7 51 if(HABO1==1 && DIRE==0)
JAgustinOtero 0:fc3d2482b2f7 52 O1=O1-(0.005);
JAgustinOtero 0:fc3d2482b2f7 53 if(HABO2==1 && DIRE==0)
JAgustinOtero 0:fc3d2482b2f7 54 O2=O2-(0.005);
JAgustinOtero 0:fc3d2482b2f7 55 }
JAgustinOtero 0:fc3d2482b2f7 56
JAgustinOtero 0:fc3d2482b2f7 57 void ME_servocontrol()
JAgustinOtero 0:fc3d2482b2f7 58 {
JAgustinOtero 0:fc3d2482b2f7 59 switch(ME_servocontrol_estado) {
JAgustinOtero 0:fc3d2482b2f7 60 case PM:
JAgustinOtero 0:fc3d2482b2f7 61 O1=0.04625;
JAgustinOtero 0:fc3d2482b2f7 62 O2=0.0025;
JAgustinOtero 0:fc3d2482b2f7 63 HABO1=0;
JAgustinOtero 0:fc3d2482b2f7 64 HABO2=0;
JAgustinOtero 0:fc3d2482b2f7 65 LR=0;
JAgustinOtero 0:fc3d2482b2f7 66 LG=1;
JAgustinOtero 0:fc3d2482b2f7 67 LA=1;
JAgustinOtero 0:fc3d2482b2f7 68 if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 || /*AnalogicoXY<ATR*/ tsi.readPercentage()<0.35 && tsi.readPercentage()>0) ME_servocontrol_estado=LEV_PATA;
JAgustinOtero 0:fc3d2482b2f7 69 break;
JAgustinOtero 0:fc3d2482b2f7 70 case LEV_PATA:
JAgustinOtero 0:fc3d2482b2f7 71 HABO1=0;
JAgustinOtero 0:fc3d2482b2f7 72 HABO2=1;
JAgustinOtero 0:fc3d2482b2f7 73 DIRE=1;
JAgustinOtero 0:fc3d2482b2f7 74 LR=1;
JAgustinOtero 0:fc3d2482b2f7 75 LG=0;
JAgustinOtero 0:fc3d2482b2f7 76 LA=1;
JAgustinOtero 0:fc3d2482b2f7 77 if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 && O2>=0.0625) {
JAgustinOtero 0:fc3d2482b2f7 78 ME_servocontrol_estado=ADE_PATA;
JAgustinOtero 0:fc3d2482b2f7 79 }
JAgustinOtero 0:fc3d2482b2f7 80 if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2>=0.0625) {
JAgustinOtero 0:fc3d2482b2f7 81 ME_servocontrol_estado=ATR_PATA;
JAgustinOtero 0:fc3d2482b2f7 82 }
JAgustinOtero 0:fc3d2482b2f7 83 if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
JAgustinOtero 0:fc3d2482b2f7 84 ME_servocontrol_estado=PM;
JAgustinOtero 0:fc3d2482b2f7 85 }
JAgustinOtero 0:fc3d2482b2f7 86 break;
JAgustinOtero 0:fc3d2482b2f7 87 case ADE_PATA:
JAgustinOtero 0:fc3d2482b2f7 88 HABO1=1;
JAgustinOtero 0:fc3d2482b2f7 89 HABO2=0;
JAgustinOtero 0:fc3d2482b2f7 90 DIRE=1;
JAgustinOtero 0:fc3d2482b2f7 91 LR=1;
JAgustinOtero 0:fc3d2482b2f7 92 LG=1;
JAgustinOtero 0:fc3d2482b2f7 93 LA=0;
JAgustinOtero 0:fc3d2482b2f7 94 if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1>=0.0625) {
JAgustinOtero 0:fc3d2482b2f7 95 ME_servocontrol_estado=BAJAR_PATA;
JAgustinOtero 0:fc3d2482b2f7 96 }
JAgustinOtero 0:fc3d2482b2f7 97 if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1>=0.0625) {
JAgustinOtero 0:fc3d2482b2f7 98 ME_servocontrol_estado=LEV_PATA;
JAgustinOtero 0:fc3d2482b2f7 99 }
JAgustinOtero 0:fc3d2482b2f7 100 if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
JAgustinOtero 0:fc3d2482b2f7 101 ME_servocontrol_estado=PM;
JAgustinOtero 0:fc3d2482b2f7 102 }
JAgustinOtero 0:fc3d2482b2f7 103 break;
JAgustinOtero 0:fc3d2482b2f7 104 case BAJAR_PATA:
JAgustinOtero 0:fc3d2482b2f7 105 HABO1=0;
JAgustinOtero 0:fc3d2482b2f7 106 HABO2=1;
JAgustinOtero 0:fc3d2482b2f7 107 DIRE=0;
JAgustinOtero 0:fc3d2482b2f7 108 LR=1;
JAgustinOtero 0:fc3d2482b2f7 109 LG=0;
JAgustinOtero 0:fc3d2482b2f7 110 LA=0;
JAgustinOtero 0:fc3d2482b2f7 111 if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O2<=0.025) {
JAgustinOtero 0:fc3d2482b2f7 112 ME_servocontrol_estado=ATR_PATA;
JAgustinOtero 0:fc3d2482b2f7 113 }
JAgustinOtero 0:fc3d2482b2f7 114 if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2<=0.025) {
JAgustinOtero 0:fc3d2482b2f7 115 ME_servocontrol_estado=ADE_PATA;
JAgustinOtero 0:fc3d2482b2f7 116 }
JAgustinOtero 0:fc3d2482b2f7 117 if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
JAgustinOtero 0:fc3d2482b2f7 118 ME_servocontrol_estado=PM;
JAgustinOtero 0:fc3d2482b2f7 119 }
JAgustinOtero 0:fc3d2482b2f7 120 break;
JAgustinOtero 0:fc3d2482b2f7 121 case ATR_PATA:
JAgustinOtero 0:fc3d2482b2f7 122 HABO1=1;
JAgustinOtero 0:fc3d2482b2f7 123 HABO2=0;
JAgustinOtero 0:fc3d2482b2f7 124 DIRE=0;
JAgustinOtero 0:fc3d2482b2f7 125 LR=0;
JAgustinOtero 0:fc3d2482b2f7 126 LG=1;
JAgustinOtero 0:fc3d2482b2f7 127 LA=0;
JAgustinOtero 0:fc3d2482b2f7 128 if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1<=0.025) {
JAgustinOtero 0:fc3d2482b2f7 129 ME_servocontrol_estado=LEV_PATA;
JAgustinOtero 0:fc3d2482b2f7 130 }
JAgustinOtero 0:fc3d2482b2f7 131 if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1<=0.025) {
JAgustinOtero 0:fc3d2482b2f7 132 ME_servocontrol_estado=BAJAR_PATA;
JAgustinOtero 0:fc3d2482b2f7 133 }
JAgustinOtero 0:fc3d2482b2f7 134 if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
JAgustinOtero 0:fc3d2482b2f7 135 ME_servocontrol_estado=PM;
JAgustinOtero 0:fc3d2482b2f7 136 }
JAgustinOtero 0:fc3d2482b2f7 137 break;
JAgustinOtero 0:fc3d2482b2f7 138 }
JAgustinOtero 0:fc3d2482b2f7 139 }