File content as of revision 41:e86f6f2e2bfc:

//GrannyStepper
#include "mbed.h"
#include "PM2_Libary.h"
#include <iostream>
using namespace std;

//Eingänge
DigitalIn user_button(PC_13);
DigitalIn mechanical_button(PC_5);

//Ausgänge
DigitalOut enable_motors(PB_15); // Motoren freischalten

// while loop gets executed every main_task_period_ms milliseconds
int main_task_period_ms = 50;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
Timer main_task_timer;          // create Timer object which we use to run the main task every main task period time in ms

//DC-Motoren
float   pwm_period_s = 0.00005f; //PWM-Periode fürd DC-Motoren definiert
FastPWM pwm_M1(PB_13); 
//Endcoder
EncoderCounter  encoder_M1(PA_6, PC_7);

// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
float k_gear = 100.0f / 78.125f;            // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1
float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1

// SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box  with default contoller parameters
//SpeedController speedController_M1(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear
//float max_speed_rps = 0.5f;                 // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage

// PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
//PositionController positionController_M1(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear, we need a different speed controller gain here

//Servomotor
Servo servo_S1(PB_12);
float servo_S1_angle = 0; 
int servo_period_mus = 20000;

int servo_counter = 0; 
int loops_per_seconds = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms)));


int main(){

    int counter_user_button = 0;
    bool counter_stopp = 0;

    pwm_M1.period(pwm_period_s);
    pwm_M1.write(0.5f);
    
    

    while(1){
        //Zähler
        if ((user_button.read() == 0) & (counter_stopp == 0)) {
            counter_user_button++;
            counter_stopp = 1;

        }
        if (user_button.read()){
            counter_stopp = 0;
        }
        if (counter_user_button >= 5){
            counter_user_button = 1;
        }

        float speed_counter;//Geschwindigkeit für DC-Motor

        switch (counter_user_button){
            case 1:
            //DC-Motor
            enable_motors = 1;
            //Servo
            servo_S1.Enable(servo_S1_angle, servo_period_mus);
            break;
            case 2:
            //DC-Motor
            speed_counter = 1.0f;
            pwm_M1.write(speed_counter);
            //Servo
            servo_S1.SetPosition(servo_S1_angle);
            if (servo_S1_angle <= 0.8f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) {
                servo_S1_angle += 0.001f;
            }
            servo_counter++;
            break; 
            case 3:
            //DC-Motor
            speed_counter = 0.5f;
            pwm_M1.write(speed_counter);
            //Servo
            servo_S1_angle = 1;
            servo_S1.SetPosition(servo_S1_angle);
            break; 
            case 4:
            //DC-Motor
            enable_motors = 0;
            //Servo
            servo_S1.Disable();
            servo_counter = 0;
            servo_S1_angle = 0;
            break; 
        }
    
        printf( "counterZähler %d   angle %f    counterServo  %d\n",counter_user_button, servo_S1_angle, servo_counter);

        
        
    }
        
}