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:
3:4f281c336a8b
Parent:
2:75b2f713161c
Child:
4:fb67a4284aaa
Child:
5:2ae500da8fe1

File content as of revision 3:4f281c336a8b:

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

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

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

//get the reference of the velocity: positive or negative
double getreferencevelocity()
{
    referencevelocity = -1.0 + 2.0*pot2.read();
    return referencevelocity;
}   

//send value to motor
void sendtomotor(float motorvalue)
{   
    motorvalue2 = motorvalue;
    vel = fabs(motorvalue);
    vel = vel > 1.0f ? 1.0f : vel;   
    motor1Velocity = vel;

    motor1Direction = (motorvalue > 0.0f);
}

// function to call reference velocity, measured velocity and controls motor with feedback
void measureandcontrol(void)
{
    float referencevelocity = getreferencevelocity();
    float measuredvelocity = getmeasuredvelocity();
    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("velocity = %f\r\n", vel);
        pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
        pc.printf("motorvalue2 = %f\r\n", motorvalue2);
    }
}