File content as of revision 11:8457b851e3e1:

#include "mbed.h"
#include "stdio.h"
#include <Timer.h>
#include "messen.h"

static Serial pc(SERIAL_TX, SERIAL_RX);
static SPI spi(PE_6,PE_5,PE_2);  //mosi,miso,sclk
static DigitalOut ncs(PE_4);    //ssel

static AnalogIn poti_1(PF_3);
static AnalogIn poti_2(PF_10);
static AnalogIn poti_3(PF_4);
static AnalogIn poti_4(PF_5);

static DigitalIn taster4(PG_4,PullDown);
static DigitalIn taster3(PD_10,PullDown);
static DigitalIn taster2(PG_14,PullDown);
static DigitalIn taster1(PF_12,PullDown);

static DigitalOut db0(PC_8);
static DigitalOut db1(PC_9);
static DigitalOut db2(PC_10);
static DigitalOut db3(PC_11);
static DigitalOut db4(PC_12);
static DigitalOut db5(PC_13);
static DigitalOut db6(PC_14);
static DigitalOut db7(PC_15);

static PwmOut Motor1 (PC_8);       //  Schwarz  QBRAIN: rot
static PwmOut Motor2 (PC_9);       //  Weiß             orange
static PwmOut Motor3 (PC_6);       //  Grau             weiß
static PwmOut Motor4 (PB_9);       //  Blau             braun
//  Gelb und Orange Vcc +5V
//  Gnd Rot



float y_off, y_high_low_summe, y_winkel;
int16_t n1, n2, n3, n4;
uint16_t i, j;

uint16_t zeit;
uint32_t zeit2;


Timer timer;
Timer timer2;


int main()
{   i = j = 0;
    //Motor auf Drehzahl null einstellen start
    n1 = n2 = n3 = n4 =700;
    Motor1.period_ms(2);
    Motor2.period_ms(2);
    Motor3.period_ms(2);
    Motor4.period_ms(2);
    wait_ms (10);

    Motor1.pulsewidth_us(n1);
    Motor2.pulsewidth_us(n2);
    Motor3.pulsewidth_us(n3);
    Motor4.pulsewidth_us(n4);
    wait_ms (10);
    pc.printf("\n\r");
    pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
    //Motor auf Drehzahl null einstellen ende
    initialisierung_gyro();
    wait_ms(20);    
    //Motor anlernen start
    if (taster3)            
    {         
        n1 = n2 = n3 = n4 = 1900;
        Motor1.pulsewidth_us(n1);
        Motor2.pulsewidth_us(n2);
        Motor3.pulsewidth_us(n3);
        Motor4.pulsewidth_us(n4);
        pc.printf("Nach einem langem PiepTon  Taste1 betaetigen\n\r");
        pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
        while (!taster4) 
        {
            if (taster1) 
            {
                n1 = n2 =n3 = n4 = 700;
            }
            if (taster2) 
            {
                n1 = n2 = n3 = n4 =1900;
            }
            Motor1.pulsewidth_us(n1);
            Motor2.pulsewidth_us(n2);
            Motor3.pulsewidth_us(n3);
            Motor4.pulsewidth_us(n4);
            pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
        }
    }
    //Motor anlernen ende  
    pc.printf("Druecke Taster1 für den Start\n\r");
    while(1)
    {
    n1 = n2 = n3 = n4 =700;
    Motor1.pulsewidth_us(n1);
    Motor2.pulsewidth_us(n2);
    Motor3.pulsewidth_us(n3);
    Motor4.pulsewidth_us(n4);
        if (taster1) 
        {
            pc.printf("Du hast den Hobel gestartert, lauf!!!\n\r");
            while(!taster4) 
            {
                pc.printf("Halte still, es wird kalibiriert!!!\n\r");
                //Offset:
                for(i = 1; i <= 40000; i++) 
                {
                    y_off += aktuell_gyro_y();
                }
                y_off /= 40000;
                pc.printf("Y_Off = %2.2f\n\r",y_off);
                pc.printf("Ich habe fertig kalibiriert!!!\n\r");
                for(i = 1; i<= 100; i++)
                {
                n2 += 1;
                n4 += 1;
                Motor4.pulsewidth_us(n4);
                Motor2.pulsewidth_us(n2);
                wait_ms(20);
                }
                wait_ms(2000);
                //Messen
                y_high_low_summe = 0;
                i = 0;
                j = 0;
                timer.reset();
                timer2.reset();
                y_winkel = 0;
                while(!taster4) {
                    i++;    //Zähler für den Printf
                    j++;    //Zähler für die Motoren
                    timer.start();
                    timer2.start();
                    zeit = timer.read_us();
                    timer.reset();
                    timer.start();
                    zeit2 = timer2.read_ms();
                    y_high_low_summe = aktuell_gyro_y() - y_off;              //Offset vom Messwert subtrahieren
                    y_winkel = y_winkel + (y_high_low_summe * zeit * 0.000001 * 1/16.4);   //Messwert multipliziert mit der Zeitdifferenz
                    if (y_winkel < 0 && j == 1000 && n4 < 1200) 
                    {
                        n4++;
                        Motor4.pulsewidth_us(n4);
                        if (n2 > 800)
                        {
                        n2 --;
                        Motor2.pulsewidth_us(n2);
                        }
                        j = 0;
                    }
                    if (y_winkel > 0 && j == 1000 && n2 < 1200) 
                    {
                        n2++;
                        if (n4 > 800)
                        {
                        n4 --;
                        Motor2.pulsewidth_us(n2);
                        }
                        Motor4.pulsewidth_us(n4);
                        j = 0;
                    }
                    if (i == 20000) 
                    {
                        pc.printf("y_Winkel: = %3.5f\tMotor2:%d%%\tMotor4:%d%%\tMotor2:%d\tMotor4:%d\n\r", y_winkel, (n2-700)*100/(1900-700), (n4-700)*100/(1900-700),n2, n4);
                        i = 0;
                    }
                }
            }
        }
    }
}








/*if(taster1.read())
{
i = 0.35*2000;
}
if(taster2.read())
{
i = 0.95*2000;
}*/


/*
        float x = aktuell_acc_x ();
        float z = aktuell_acc_z ();
        winkel1 = (((float)atan2(x, z)));
        float y = aktuell_acc_y ();
        winkel2 = (((float)atan2(y, z)));

        pc.printf("%4.2f\t\t\t%4.2f\t\r",winkel1*360/6.28, winkel2*360/6.28);*/