File content as of revision 0:b0f9c5ac0305:

#include "motor_mixer.h"

RawSerial pc_motor_mixer(USBTX, USBRX);
PwmOut motor1_out(D5);
PwmOut motor2_out(D6);
PwmOut motor3_out(D9);
PwmOut motor4_out(D10);

//Definition der Ausgänge für die Motoren
/*
                FRONT

    CW M3(D9)           CCW M1(D5)
        \                   /
         \                 /
          -----------------      
         /                 \
        /                   \
    CCW M2(D6)          CW M4(D10)
    
                BACK

motor1 = rcgas + roll - pitch + yaw
motor2 = rcgas - roll + pitch + yaw
motor3 = rcgas − roll - pitch - yaw
motor4 = rcgas + roll + pitch - yaw
*/

void motormixer::motor_all_period(int period)
{
    motor1_out.period_ms(period);
    motor2_out.period_ms(period);
    motor3_out.period_ms(period);
    motor4_out.period_ms(period);
}


void motormixer::motor_all_off()
{
    motor1_out.pulsewidth_us(1100);
    motor2_out.pulsewidth_us(1100);
    motor3_out.pulsewidth_us(1100);
    motor4_out.pulsewidth_us(1100);
}

/////////////////////////MOTOR1//////////////////////////////////////////////////////////////////////////////////////////
void motormixer::motor1_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
{

    int motor1_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready - pitch_error_pwm_ready + yaw_error_pwm_ready;

    //Wert an Motor-Regler senden
    if (motor1_pwm_ready < 1200) {motor1_out.pulsewidth_us(1200);}
    else if (motor1_pwm_ready > 1759) {motor1_out.pulsewidth_us(1759);}
    
    else {

        motor1_out.pulsewidth_us(motor1_pwm_ready);
        //pc_motor_mixer.printf("Output Motor1: %d\n", motor1_pwm_ready);
    }
}

/////////////////////////MOTOR2//////////////////////////////////////////////////////////////////////////////////////////
void motormixer::motor2_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
{

    int motor2_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready + pitch_error_pwm_ready + yaw_error_pwm_ready;

    //Wert an Motor-Regler senden
    if (motor2_pwm_ready < 1200) {motor2_out.pulsewidth_us(1200);}
    else if (motor2_pwm_ready > 1759) {motor2_out.pulsewidth_us(1759);}
    
    else {

        //pc_motor_mixer.printf("Output Motor2: %d\n", motor2_pwm_ready);
        motor2_out.pulsewidth_us(motor2_pwm_ready);
    }
}

/////////////////////////MOTOR3//////////////////////////////////////////////////////////////////////////////////////////
void motormixer::motor3_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
{

    int motor3_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready - pitch_error_pwm_ready - yaw_error_pwm_ready;

    //Wert an Motor-Regler senden
    if (motor3_pwm_ready < 1200) {motor3_out.pulsewidth_us(1200);}
    else if (motor3_pwm_ready > 1759) {motor3_out.pulsewidth_us(1759);}
    
    else {

        motor3_out.pulsewidth_us(motor3_pwm_ready);
        //pc_motor_mixer.printf("Output Motor3: %d\n", motor3_pwm_ready);
    }
}

/////////////////////////MOTOR4//////////////////////////////////////////////////////////////////////////////////////////
void motormixer::motor4_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
{

    int motor4_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready + pitch_error_pwm_ready - yaw_error_pwm_ready;

    //Wert an Motor-Regler senden
    if (motor4_pwm_ready < 1200) {motor4_out.pulsewidth_us(1200);}
    else if (motor4_pwm_ready > 1759) {motor4_out.pulsewidth_us(1759);}
    
    else {

        //pc_motor_mixer.printf("Output Motor4: %d\n", motor4_pwm_ready);
        motor4_out.pulsewidth_us(motor4_pwm_ready);
    }
}

/////////////////////////MOTOREN KALIBRIEREN//////////////////////////////////////////////////////////////////////////////////////////
void motormixer::motor_calibration(int period)
{
    pc_motor_mixer.printf("Periode einstellen\n");
    motor1_out.period_ms(period);
    motor2_out.period_ms(period);
    motor3_out.period_ms(period);
    motor4_out.period_ms(period);
    pc_motor_mixer.printf("Done\n");

    pc_motor_mixer.printf("Vollast...3Sec warten...\n");
    motor1_out.pulsewidth_us(1759);
    motor2_out.pulsewidth_us(1759);
    motor3_out.pulsewidth_us(1759);
    motor4_out.pulsewidth_us(1759);
    
    wait(5);
    
    pc_motor_mixer.printf("Keine Last...3Sec warten...\n");
    motor1_out.pulsewidth_us(1111);
    motor2_out.pulsewidth_us(1111);
    motor3_out.pulsewidth_us(1111);
    motor4_out.pulsewidth_us(1111);
    pc_motor_mixer.printf("Done\n");
    
    wait(5);
}