Alle functies van de motoren werken

Dependencies:   MODSERIAL QEI mbed

Fork of worknotjet by Timo de Vries

main.cpp

Committer:
Frostworks
Date:
2016-10-17
Revision:
5:353ed56417a2
Parent:
4:7b8bf798bb72
Child:
6:4e4df2f6157e

File content as of revision 5:353ed56417a2:

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

DigitalOut M1_rotate(D2); // voltage only base rotation
PwmOut M1_Speed(D3);      // voltage only base rotation

DigitalOut M2_rotate(D4);   // encoder side pot 2 translation
PwmOut M2_Speed(D5);        // encoder side pot 2 translation

DigitalOut M3_rotate(D7);   // encoder side pot 1 spatel rotation
PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation

DigitalIn links(SW3);
DigitalIn rechts(SW2);
//DigitalOut M2_rotate(D6);
//PwmOut M2_Speed(D7);

AnalogIn pot1(A4); // pot 1 motor 1
AnalogIn pot2(A3); // pot 2 motor 3

MODSERIAL pc(USBTX, USBRX);
bool draairechts;
bool draailinks;
bool turnthedamthing = 0;
float waiter = 0.1;
void GetDirections()
{
    pc.baud(115200);
    if ((rechts == 0) && (links == 0) && (turnthedamthing == 0)) {          
        draailinks = 0;
        draairechts = 0;
        turnthedamthing = 1;
        pc.printf("begin de actie \n \r ");
        wait(waiter);

    } else if ((rechts == 0) && (links == 0) && (turnthedamthing == 1)) {
        draailinks = 0;
        draairechts = 0;
        turnthedamthing = 0;
        pc.printf("breek de actie af \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (links == 1)) {

    } else if ((rechts == 1) && (draailinks == 0)) {
        /* if the right button is pressed and the motor isn't rotating to the left,
        then start rotating to the right etc*/
        draairechts = !draairechts;
        pc.printf("draai naar rechts \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (draailinks == 1)) {
        draailinks = 0;
        draairechts = !draairechts;
        pc.printf("draai naar rechts na links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 0)) {
        draailinks = !draailinks;
        pc.printf("draai naar links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 1)) {
        draairechts = 0;
        draailinks = !draailinks;
        pc.printf("draai naar links na rechts \n \r ");
        wait(waiter);
    }
    wait(2*waiter);
}
int main()
{


    /*
        float GetReferenceVelocity() {
            // Returns reference velocity in rad/s.
            // Positive value means clockwise rotation.
            const float maxVelocity=8.4; // in rad/s of course!    float referenceVelocity;  // in rad/s
            if (a)   {
                // Clockwise rotation
                referenceVelocity = potMeterIn * maxVelocity;
                
            } else {
                // Counterclockwise rotation
                referenceVelocity = -1*potMeterIn * maxVelocity;
                
            }
            return referenceVelocity;
        }

        void SetMotor1(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor1DirectionPin=1;
            } else {
                motor1DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor1MagnitudePin = 1;
            } else {
                motor1MagnitudePin = fabs(motorValue);
            }
        }

        void SetMotor2(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor2DirectionPin=1;
            } else {
                motor2DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor2MagnitudePin = 1;
            } else {
                motor2MagnitudePin = fabs(motorValue);
            }
        }

        void SetMotor3(float motorValue) {
            // Given -1<=motorValue<=1, this sets the PWM and direction
            // bits for motor 1. Positive value makes motor rotating
            // clockwise. motorValues outside range are truncated to
            // within range
            if (motorValue >=0) {
                motor3DirectionPin=1;
            } else {
                motor3DirectionPin=0;
            }
            if (fabs(motorValue)>1) {
                motor3MagnitudePin = 1;
            } else {
                motor3MagnitudePin = fabs(motorValue);
            }
        }
        */

    while (true) {
        GetDirections();
        if (draairechts == true) {
            M1_Speed = 0.5;
            M1_rotate = 0;
        } else if (draailinks == true) {
            M1_Speed = 0.5;
            M1_rotate = 1;
        } else if ((draailinks == false) && (draairechts == false)) {
            M1_Speed = 0;
        }



        /*
        if ((draailinks == true) && (draairechts == false)) {
            M1_Speed = 0.5;
            M1_rotate = 0;
        } else if ((draairechts == true) && (draailinks == false)) {
            M1_Speed = 0.5;
            M1_rotate = 1;
        } else {
            M1_Speed = 0;
        }
        */

        /*
        float M2 = M2_rotate.read();
        float potje1 = pot1.read();
        float potje2 = pot2.read();
        M1_Speed.write(potje1);
        M3_Speed.write(potje2);
        wait(0.1);
        pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
        */

    }

}