main.cpp

00001 #include "mbed.h"
00002 Serial pc(USBTX, USBRX);
00003 
00004 DigitalInOut pinReference(p13);
00005 DigitalOut driveLine2(p14);
00006 AnalogIn senseLine3(p15);
00007 
00008 DigitalOut red1(p21);
00009 DigitalOut red2(p22);
00010 DigitalOut red3(p23);
00011 DigitalOut red4(p24);
00012 
00013 DigitalOut blue1(p28);
00014 DigitalOut blue2(p27);
00015 DigitalOut blue3(p26);
00016 DigitalOut blue4(p25);
00017 
00018 int main() {
00019     while(1) {
00020 
00021        pinReference.output();
00022        pinReference = 0;
00023        driveLine2 = 1;
00024        float force = 0;
00025        force = senseLine3.read();
00026        wait(0.001);
00027        pinReference.input();
00028        driveLine2 = 0;
00029        float position = 0;
00030        position = senseLine3.read();
00031        pc.printf("Force: %f", force);
00032        pc.printf("     Position: %f\r\n", position);
00033        wait(0.001);
00034        
00035        if( position ==0 ) {
00036            red1 = red2 = red3 = red4 = 0;
00037             wait(0.001);
00038            }else if( position < 0.35 ) {
00039            red1 = 1;        
00040            red2 = red3 = red4 = 0;
00041             wait(0.001);
00042            } else if (position >= 0.35 && position < 0.62 ) {
00043                red1 = red2 = 1;
00044                red3 = red4 = 0;
00045                 wait(0.001);
00046                } else if (position >= 0.62 && position < 0.9 ) {
00047                    red1 = red2 = red3 = 1;
00048                    red4 = 0;
00049                     wait(0.001);
00050                    } else if ( position >= 0.9) {
00051                        red1 = red2 = red3 = red4 = 1;
00052                         wait(0.001);
00053                         
00054                        }
00055         if ( position ==0 ) {
00056            red1 = red2 = red3 = red4 = blue1 = blue2 = blue3 = blue4 = 0;
00057             wait(0.001);
00058            }else if( position < 0.35 && force >=0.8) {
00059            red1 = red2 = red3 = red4 = 0;
00060            blue2 = blue3 = blue4 = 0;
00061            blue1 = 1;
00062             wait(0.001);
00063            } else if (position >= 0.35 && position < 0.62 && force >=0.8 ) {
00064                 red1 = red2 = red3 = red4 = 0;
00065                 blue3 = blue4 = 0;
00066                 blue1 = blue2 = 1;
00067                 wait(0.001);
00068                } else if (position >= 0.62 && position < 0.9 && force >=0.8) {
00069                    red1 = red2 = red3 = red4 = 0;
00070                    blue4 = 0;
00071                    blue1 = blue2 = blue3 = 1;                 
00072                     wait(0.001);
00073                    } else if ( position >= 0.9 && force >=0.8) {
00074                        red1 = red2 = red3 = red4 = 0;
00075                        blue1 = blue2 = blue3 = blue4 = 1;   
00076                         wait(0.001);
00077                        }
00078         wait(0.3);
00079         
00080     }
00081 }