inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Project_script by Marijke Zondag

main.cpp

Committer:
MarijkeZondag
Date:
2018-10-01
Revision:
6:f4bbb73f3989
Parent:
5:107eb09fb095
Child:
7:f32005d13749

File content as of revision 6:f4bbb73f3989:

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

DigitalOut directionpin1(D4);
PwmOut pwmpin1(D5);
AnalogIn potmetervalue1(A1);
DigitalIn button2(D9);          //klopt dit?

DigitalOut directionpin2(D7);
PwmOut pwmpin2(D6);
AnalogIn potmetervalue2(A2);
DigitalIn button1(D10);          //klopt dit?

MODSERIAL pc(USBTX, USBRX);

int main()
{ 
    pc.baud(115200);
    pc.printf("hello\n\r");
    pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 

    
    while (true)
    {
          
          float u1 = potmetervalue1;
          float u2 = potmetervalue2;
          
          float m1 = ((u1*2.0f)-1.0f);
          float m2 = ((u2*2.0f)-1.0f);
          
         // pc.printf("Motor value is %f\n\r",m);

          /*if (m <= -0.005) 
            {
                directionpin.write(0);
                pwmpin = fabs(m); //pwm duty cycle can only be positive, floating 
                pwmpin.period_us(60); //60 microseconds PWM period, 16.7 kHz 
                pc.printf("Motor draait naar A\n\r");
            }
            
          else if (m >= 0.005)
            { 
                directionpin.write(1);
                pwmpin = fabs(m);
                pwmpin.period_us(60); //60 microseconds PWM period, 16.7 kHz
                pc.printf("Motor draait naar B\n\r");

            }
        
          else if (-0.4 < m && m < 0.)
            {
                //pwmpin = 0.0;
                pwmpin.period_us(60); //60 microseconds PWM period, 16.7 kHz
                pc.printf("Motor zou moeten stoppen.\n\r");
            }
            
            Met deze code kunnen we serial communication gebruiken. Als we dit niet nodig hebben en alleen de motor willen aansturen, dan gebruiken we de code hieronder.
            */
        
        pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
        directionpin1.write(m1>0);        //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
        wait(0.001f);                   //zodat de code niet oneindig doorgaat.

        pwmpin2 = fabs(m2*0.6f)+0.4f;    
        directionpin2.write(m2>0);        
        wait(0.001f);
            
    }
}