Motor steering program

Dependencies:   mbed

main.cpp

Committer:
johnyp1845
Date:
2017-06-02
Revision:
1:797c7fc3a9b8
Parent:
0:26a4609c804e

File content as of revision 1:797c7fc3a9b8:

#include "mbed.h"

int period = 18;//how long the cycles are, Can be changed to increase accuracy
int cycles = 5;// Number of cycles sampled, More cycles = more accuracy but more latency 
float cutOff = .268; //rms value for starting breaks 
float hysteresis = .05;// undershoot value










DigitalOut clamp(D7);
AnalogIn voltage(A1);
int count, countCutOff;
float value;
double squareValue, RMSvalue, sum, RMS, x;

double getRMS();
int main() 
{
    while(1) 
    {
    RMS =  getRMS(); // gets values
    if(RMS>=cutOff) // checks to see above threshold
    {
        clamp = 1;
        
    } 
    else
    {
        clamp = 0; 
    }
         while(clamp == 1)
      {
          RMS = getRMS();
          x = cutOff * hysteresis;//calculates offset
          x = cutOff - x;// sets offset
          if(RMS <= x)//checks for undervalue
          {
              clamp = 0;
          }
          else
          {
              clamp = 1;
          }
       }      
    }
    
}
double getRMS()
{
    sum = 0;//resets sum and count
    count = 0;
    countCutOff = period * cycles;
    countCutOff = countCutOff * 10;
    while(count<=countCutOff)
    {
    value = 3.3*voltage.read(); //reads voltage
    squareValue = value*value; // squares value
    sum = sum + squareValue;// sums value in preperation for RMS
    count++; //increase sample counter
    wait_us(100);// wait
    }
    sum = sum/countCutOff; // averages sum
    RMSvalue = sqrt(sum); // Creates RMS value
    return RMSvalue;
}