10/25/2015

Dependencies:   PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot

main.cpp

Committer:
palmdotax
Date:
2015-10-23
Revision:
3:365615fa646e
Parent:
2:933d3edf38da
Child:
4:e6ab360e7de6

File content as of revision 3:365615fa646e:

#include"mbed.h"
#include "move.h"
#include "PID.h"

DigitalOut led1(LED1);
InterruptIn encoderA(D6);
InterruptIn encoderB(D5);
Timer timerStart;
Timeout timecount;
move m1;
PID P1(0.001,0,0,0.1);

//DigitalIn encoderB(D5);


Serial pc(SERIAL_TX,SERIAL_RX);
 int Encoderpos = 0;
 float valocity =0,pulse=0,count=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;
  if(pulse>=128)
  {count+=1;pulse=0;}
}
  

/*void EncoderB()
{ 
    if(encoderA==1)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pc.printf("%d",Encoderpos);
}*/
void getRPM()
{
    valocity=count;
    count=0;
}

int main()
{
   
    pc.baud(115200);
    encoderA.rise(&EncoderA);
    timerStart.start();
    
   
       P1.setMode(1);
       P1.setProcessValue(0.7);
       P1.setSetPoint(0.8);
       P1.setBias(0);
       pc.printf("READY \n");
    while(1)
    
    {  
       led1=1;
       
       timecount.attach(&getRPM, 60);
       // m1.movespeed(1,1.0);
       // outPID=P1.compute();
       // pc.printf("%f  \n",valocity);
        wait(0.1);
    
       // pc.printf("%d \n",Encoderpos);
         
    }  
}