![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
10/25/2015
Dependencies: PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot
main.cpp
- Committer:
- palmdotax
- Date:
- 2015-12-14
- Revision:
- 8:8fbc0c858875
- Parent:
- 7:2daffd310c71
- Child:
- 9:86701fec3f79
File content as of revision 8:8fbc0c858875:
#include"mbed.h" #include "move.h" #include "PID.h" DigitalOut led1(LED1); InterruptIn encoderA(D10); InterruptIn encoderB(D11); Timer timerStart; Timeout timecount; move m1; PID P1(0.005,0.005,0,0.1); double setp1=0,setp2=0; //DigitalIn encoderB(D5); int times; Serial pc(SERIAL_TX,SERIAL_RX); int Encoderpos = 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; void EncoderA() { if(encoderB==0) { Encoderpos = Encoderpos + 1;} else { Encoderpos = Encoderpos -1;} //Encoderpos = Encoderpos + 1; // valocity+=1; // pc.printf("%d \n",Encoderpos); pulse+=1; //pc.printf("pulse=%f \n",pulse); // if(pulse==128) //{count+=1;pulse=0; pc.printf("count=%f \n",count);} } /*void EncoderB() { if(encoderA==1) { Encoderpos = Encoderpos + 1;} else { Encoderpos = Encoderpos -1;} pc.printf("%d",Encoderpos); }*/ void getvelo() { valocity=pulse*((2*3.14*r)/128); pc.printf("valocity=%f \n",valocity); count=0; timerStart.reset(); } double map(double x, double in_min, double in_max, double out_min, double out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void PID_m1() { setp1=map(1.0,0.0,1.094,0.0,1.0); P1.setSetPoint(setp1); times=timerStart.read(); if(times==1)// m/s { getvelo(); //pc.printf("TIME \n"); times=0; pulse=0; } P1.setProcessValue(valocity); outPID=P1.compute(); //pc.printf("outPID=%f \n",outPID); m1.movespeed_1(1,setp1,outPID); } void PID_m2() { setp2=map(1.0,0.0,1.094,0.0,1.0); P1.setSetPoint(setp2); times=timerStart.read(); if(times==1)// m/s { getvelo(); //pc.printf("TIME \n"); times=0; pulse=0; } P1.setProcessValue(valocity); outPID=P1.compute(); //pc.printf("outPID=%f \n",outPID); m1.movespeed_2(1,setp2,outPID); } int main() { int times=0; pc.baud(115200); encoderA.rise(&EncoderA); timerStart.start(); P1.setMode(1); //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(setp);//setpont P1.setBias(0); pc.printf("READY \n"); led1=1; while(1) { /* // pc.printf("%f \n",setp); times=timerStart.read(); if(times==1)// m/s { getvelo(); //pc.printf("TIME \n"); times=0; pulse=0; } P1.setProcessValue(valocity); // m1.movespeed(1,1.0); outPID=P1.compute(); pc.printf("outPID=%f \n",outPID); m1.movespeed(1,setp,outPID); // m1.movespeed(1,1,0); wait(0.1); */ } }