Betago / Mbed 2 deprecated BETAGO

10/25/2015

Dependencies:   PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot

main.cpp@6:9ed82a812ece, 2015-10-27 (annotated)

Committer:
palmdotax
Date:
Tue Oct 27 05:45:49 2015 +0000
Revision:
6:9ed82a812ece
Parent:
5:68b740d113e6
Child:
7:2daffd310c71
27/10/2015;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
palmdotax 0:f02641e3fb91 1 #include"mbed.h"
palmdotax 1:7c3dbf140bfc 2 #include "move.h"
palmdotax 0:f02641e3fb91 3 #include "PID.h"
palmdotax 1:7c3dbf140bfc 4
palmdotax 3:365615fa646e 5 DigitalOut led1(LED1);
palmdotax 4:e6ab360e7de6 6 InterruptIn encoderA(D10);
palmdotax 4:e6ab360e7de6 7 InterruptIn encoderB(D11);
palmdotax 2:933d3edf38da 8 Timer timerStart;
palmdotax 3:365615fa646e 9 Timeout timecount;
palmdotax 1:7c3dbf140bfc 10 move m1;
palmdotax 4:e6ab360e7de6 11 PID P1(0.005,0.005,0,0.1);
palmdotax 6:9ed82a812ece 12 float setp=0;
palmdotax 0:f02641e3fb91 13 //DigitalIn encoderB(D5);
palmdotax 0:f02641e3fb91 14
palmdotax 0:f02641e3fb91 15
palmdotax 0:f02641e3fb91 16 Serial pc(SERIAL_TX,SERIAL_RX);
palmdotax 0:f02641e3fb91 17 int Encoderpos = 0;
palmdotax 6:9ed82a812ece 18 float valocity =0,pulse=0,count=0,r=0.125,velocityreal=0;
palmdotax 3:365615fa646e 19 float outPID =0;
palmdotax 3:365615fa646e 20 //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;
palmdotax 3:365615fa646e 21
palmdotax 0:f02641e3fb91 22
palmdotax 0:f02641e3fb91 23 void EncoderA()
palmdotax 0:f02641e3fb91 24 { if(encoderB==0)
palmdotax 0:f02641e3fb91 25 { Encoderpos = Encoderpos + 1;}
palmdotax 0:f02641e3fb91 26 else
palmdotax 0:f02641e3fb91 27 { Encoderpos = Encoderpos -1;}
palmdotax 1:7c3dbf140bfc 28
palmdotax 0:f02641e3fb91 29 //Encoderpos = Encoderpos + 1;
palmdotax 3:365615fa646e 30 // valocity+=1;
palmdotax 1:7c3dbf140bfc 31 // pc.printf("%d \n",Encoderpos);
palmdotax 3:365615fa646e 32 pulse+=1;
palmdotax 5:68b740d113e6 33 //pc.printf("pulse=%f \n",pulse);
palmdotax 5:68b740d113e6 34 // if(pulse==128)
palmdotax 5:68b740d113e6 35 //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
palmdotax 5:68b740d113e6 36
palmdotax 0:f02641e3fb91 37 }
palmdotax 0:f02641e3fb91 38
palmdotax 0:f02641e3fb91 39
palmdotax 3:365615fa646e 40 /*void EncoderB()
palmdotax 0:f02641e3fb91 41 {
palmdotax 0:f02641e3fb91 42 if(encoderA==1)
palmdotax 0:f02641e3fb91 43 { Encoderpos = Encoderpos + 1;}
palmdotax 0:f02641e3fb91 44 else
palmdotax 0:f02641e3fb91 45 { Encoderpos = Encoderpos -1;}
palmdotax 0:f02641e3fb91 46 pc.printf("%d",Encoderpos);
palmdotax 3:365615fa646e 47 }*/
palmdotax 5:68b740d113e6 48 void getvelo()
palmdotax 3:365615fa646e 49 {
palmdotax 6:9ed82a812ece 50 valocity=pulse*((2*3.14*r)/128);
palmdotax 6:9ed82a812ece 51 // pc.printf("%f \n",valocity);
palmdotax 3:365615fa646e 52 count=0;
palmdotax 4:e6ab360e7de6 53 timerStart.reset();
palmdotax 0:f02641e3fb91 54 }
palmdotax 6:9ed82a812ece 55 float map(long x, long in_min, long in_max, long out_min, long out_max)
palmdotax 5:68b740d113e6 56 {
palmdotax 6:9ed82a812ece 57 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
palmdotax 5:68b740d113e6 58
palmdotax 5:68b740d113e6 59 }
palmdotax 0:f02641e3fb91 60
palmdotax 0:f02641e3fb91 61 int main()
palmdotax 0:f02641e3fb91 62 {
palmdotax 5:68b740d113e6 63 int times=0;
palmdotax 6:9ed82a812ece 64
palmdotax 5:68b740d113e6 65 pc.baud(115200);
palmdotax 5:68b740d113e6 66 encoderA.rise(&EncoderA);
palmdotax 5:68b740d113e6 67 timerStart.start();
palmdotax 2:933d3edf38da 68 P1.setMode(1);
palmdotax 6:9ed82a812ece 69 //setp=map(1,0,1.0916,0,1);
palmdotax 6:9ed82a812ece 70 setp=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 6:9ed82a812ece 71
palmdotax 6:9ed82a812ece 72 // P1.setSetPoint(velocityreal);//setpont
palmdotax 6:9ed82a812ece 73 P1.setSetPoint(1);//setpont
palmdotax 2:933d3edf38da 74 P1.setBias(0);
palmdotax 3:365615fa646e 75 pc.printf("READY \n");
palmdotax 6:9ed82a812ece 76
palmdotax 4:e6ab360e7de6 77 led1=1;
palmdotax 0:f02641e3fb91 78 while(1)
palmdotax 3:365615fa646e 79
palmdotax 2:933d3edf38da 80 {
palmdotax 6:9ed82a812ece 81 pc.printf("%l \n",setp);
palmdotax 5:68b740d113e6 82 times=timerStart.read();
palmdotax 5:68b740d113e6 83 if(times==1)// m/s
palmdotax 5:68b740d113e6 84 {
palmdotax 5:68b740d113e6 85 getvelo();
palmdotax 5:68b740d113e6 86 //pc.printf("TIME \n");
palmdotax 5:68b740d113e6 87 times=0;
palmdotax 6:9ed82a812ece 88 pulse=0;
palmdotax 4:e6ab360e7de6 89 }
palmdotax 6:9ed82a812ece 90
palmdotax 4:e6ab360e7de6 91 P1.setProcessValue(valocity);
palmdotax 3:365615fa646e 92 // m1.movespeed(1,1.0);
palmdotax 4:e6ab360e7de6 93 outPID=P1.compute();
palmdotax 6:9ed82a812ece 94 //m1.movespeed(1,velocityreal,outPID);
palmdotax 6:9ed82a812ece 95 m1.movespeed(1,1,0);
palmdotax 4:e6ab360e7de6 96
palmdotax 2:933d3edf38da 97 wait(0.1);
palmdotax 1:7c3dbf140bfc 98
palmdotax 0:f02641e3fb91 99 // pc.printf("%d \n",Encoderpos);
palmdotax 0:f02641e3fb91 100
palmdotax 0:f02641e3fb91 101 }
palmdotax 0:f02641e3fb91 102 }