![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
10/25/2015
Dependencies: PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot
Diff: main.cpp
- Revision:
- 6:9ed82a812ece
- Parent:
- 5:68b740d113e6
- Child:
- 7:2daffd310c71
--- a/main.cpp Sun Oct 25 12:03:21 2015 +0000 +++ b/main.cpp Tue Oct 27 05:45:49 2015 +0000 @@ -9,13 +9,13 @@ Timeout timecount; move m1; PID P1(0.005,0.005,0,0.1); - +float setp=0; //DigitalIn encoderB(D5); Serial pc(SERIAL_TX,SERIAL_RX); int Encoderpos = 0; - float valocity =0,pulse=0,count=0,r=0.01,velocityreal=0; + float valocity =0,pulse=0,count=0,r=0.125,velocityreal=0; float outPID =0; //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0; @@ -47,44 +47,52 @@ }*/ void getvelo() { - valocity=pulse+((2*3.14*r)/128); - pc.printf("%f \n",valocity); + valocity=pulse*((2*3.14*r)/128); + // pc.printf("%f \n",valocity); count=0; timerStart.reset(); } -void getveloreal(int velo) +float map(long x, long in_min, long in_max, long out_min, long out_max) { - velocityreal=velo; + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } int main() { int times=0; + pc.baud(115200); encoderA.rise(&EncoderA); timerStart.start(); P1.setMode(1); - getveloreal(50); - P1.setSetPoint(velocityreal);//setpont + //setp=map(1,0,1.0916,0,1); + setp=map(1.0,0.0,1.094,0.0,1.0); + + // P1.setSetPoint(velocityreal);//setpont + P1.setSetPoint(1);//setpont P1.setBias(0); pc.printf("READY \n"); + led1=1; while(1) { + pc.printf("%l \n",setp); times=timerStart.read(); if(times==1)// m/s { getvelo(); //pc.printf("TIME \n"); times=0; + pulse=0; } - //timecount.attach(&getRPM, 60); + P1.setProcessValue(valocity); // m1.movespeed(1,1.0); outPID=P1.compute(); - m1.movespeed(1,velocityreal,outPID); + //m1.movespeed(1,velocityreal,outPID); + m1.movespeed(1,1,0); wait(0.1);