Robert Schulte / Mbed 2 deprecated frdm_Motor_V2_3

Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Margreeth de Breij

main.cpp

Committer:
Rvs94
Date:
2015-09-29
Revision:
8:69bde5e32dbf
Parent:
7:67b50d4fb03c
Child:
9:774fc3c6a39e

File content as of revision 8:69bde5e32dbf:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
 
Serial pc(USBTX, USBRX); // tx, rx
DigitalOut led(LED_RED);
DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D5);
DigitalIn button1(SW3);
DigitalIn EncoderA(D3);
DigitalIn EncoderB(D2);
QEI Encoder(D3, D2, NC, 128);
HIDScope scope(3);
Ticker ScopeTime;

double Aantal_Degs;
double Aantal_pulses;
double Error;
double refference;
const double Kp = 0.007;


void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
{
    scope.set(0, motor2direction.read());
    scope.set(1, motor2speed.read());
    scope.set(2, Aantal_Degs);
    Aantal_Degs = Encoder.getPulses()*360/128/131;

    scope.send();
    
}


int main()
{
    motor2direction = 0;
    motor2speed = 0;
    led = 1;
    pc.baud(115200);
    refference = 0;
    pc.printf("Tot aan loop werkt\n");
    ScopeTime.attach_us(&ScopeSend, 10e4);
    
    
    while (true) 
    {
        
        char c = pc.getc();   
        if(c == 'r')
        {
            refference = refference + 10;
            pc.printf("rx \n");
            Error = refference - Aantal_Degs;
            while(abs(Error) > 1)
                {
                    Error = refference - Aantal_Degs;
                    motor2speed = Kp*abs(Error);
                    pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
                    if(Error > 0)
                    {
                        motor2direction = 0;
                    }
                    else
                    {
                        motor2direction = 1;
                    }
                }
        }
        if(c == 'f')
        {
            refference = refference - 10;
            pc.printf("rx \n");
            Error = refference - Aantal_Degs;
            while(abs(Error) > 1)
                {
                    Error = refference - Aantal_Degs;
                    motor2speed = Kp*abs(Error);
                    pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
                    if(Error > 0)
                    {
                        motor2direction = 0;
                    }
                    else
                    {
                        motor2direction = 1;
                    }
                }
            }
    }
}