Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp

Committer:
paulstuiver
Date:
2019-10-07
Revision:
2:75b2f713161c
Parent:
1:b862262a9d14
Child:
3:4f281c336a8b

File content as of revision 2:75b2f713161c:

#include "mbed.h"
#include "MODSERIAL.h"
#include "FastPWM.h"

DigitalIn button1(D12);
AnalogIn pot2(A0);
Ticker john;
DigitalOut motor1Direction(D6);
FastPWM motor1Velocity(D7);
Serial pc(USBTX, USBRX);
volatile double frequency = 17000.0;
volatile double period_signal = 1.0/frequency;

//get the measured velocity
double getmeasuredvelocity()
{   
    float measuredvelocity;
    measuredvelocity = pot2.read();
    return measuredvelocity;
}

//get the reference of the velocity: positive or negative
double getreferencevelocity()
{
    //const float maxvelocity = 8.4;
    /*volatile float referencevelocity;
    if (button1.read() == 0)
    {
        referencevelocity = pot2.read()*maxvelocity;
    }
    else
    {
        referencevelocity = -1*pot2.read()*maxvelocity;
    }*/
    float referencevelocity = -1.0 + 2.0*pot2.read();
    //pc.printf("The reference velocity is %f\r\n", referencevelocity);
    return referencevelocity;
}   

float vel = 0.0f;
//send value to motor
void sendtomotor(float motorvalue)
{   
    /*
    pc.printf("The motor value is %f\r\n", motorvalue);
    if (motorvalue >= 0)
    {
        motor1Direction = 1;
        pc.printf("hello");
    }
    else 
    {
        motor1Direction = 0;
        pc.printf("wow");
    }
    if (fabs(motorvalue)>1)
    {
        motor1Velocity = 1;
    }
    else
    {
        motor1Velocity = fabs(motorvalue);
    }*/
    vel = fabs(motorvalue);
     vel = vel > 1.0f ? 1.0f : vel;   
    motor1Velocity = vel;

    motor1Direction = (motorvalue > 0.0f);
    //pc.printf("The motor value is %f\r\n", motorvalue);
    //pc.printf("The motor direction is %s\r\n", motor1Direction ? "links" : "rechts");
}

// function to call reference velocity, measured velocity and controls motor with feedback
void measureandcontrol(void)
{
    float referencevelocity = getreferencevelocity();
    float measuredvelocity = getmeasuredvelocity();
//    float motorvalue = feedbackcontrol(referencevelocity - measuredvelocity);
    sendtomotor(referencevelocity);
}
int main()
{
    pc.baud(115200);
    pc.printf("Starting...\r\n");
    motor1Velocity.period(period_signal);
    john.attach(measureandcontrol, 0.001f);
    while(true)
    {
        wait(0.5);
        pc.printf("%f\r\n", vel);
    }
}