logic control

Dependencies:   Motor mbed

main.cpp

Committer:
Ty
Date:
2015-02-19
Revision:
0:c89dab5f3e1c

File content as of revision 0:c89dab5f3e1c:

#include "mbed.h"
#include "Motor.h"
Serial pc(USBTX,USBRX);//allows communication through the USB
AnalogIn range(p19);
float height;
float rv;
float dh;
float er;
Motor m(p26, p29, p30);
int counter;
int main()
{

    m.speed(0);
    while(1) {
        counter=0;
        m.speed(0);
        rv=range;
        height=(-75.7457492123552*pow(rv,2))+(90.6774745565381*rv)+(-4.00358328437282);
        pc.printf("%f,%f\n",rv,height);
        pc.printf("Enter desired height\n");
        pc.scanf("%f",&dh);
        while(counter<800){
                       rv=range;
            height=(-75.7457492123552*pow(rv,2))+(90.6774745565381*rv)+(-4.00358328437282);
            er=dh-height;
            pc.printf("%f\n",er);
            if(er>0) {
                m.speed(-.5);
            } 
            if(er<0) {
                m.speed(.8);
            }

        counter = counter+1;

        }//end while
    }
}