File content as of revision 36:1b7a89f39563:

//GrannyStepper
#include "mbed.h"
#include "PM2_Libary.h"
#include <iostream>
#include <functional>

using namespace std;

//inputs

    AnalogIn ir_analog_in_front(PC_2);  // define analog input ir sensor front pin PC_2
    AnalogIn ir_analog_in_back(PC_3);   // define analog input ir sensor back pin PC_3

    DigitalIn endswitch_step(PC_6);         // define digital input endswitch step pin PC_6
    DigitalIn endswitch_spindle_f(PB_2);    // define digital input endswitch spindle front pin PB_2 
    DigitalIn endswitch_spindle_b(PC_8);     // define digital input endswitch spindle back pin PC_8



    // goal of distance front 51mm - 14mm + 100mm + 10mm = 147mm = aprox 830 mV / 0.83 V
    // goal of distance back XX mm - 14mm + 100mm + 10mm = X mm = aprox XXX mV / X.XX V
    // 37mm = 3-3.1V

    float ir_front_mV = ir_analog_in_front.read() / 1000;   //define variable to help function
    float ir_back_mV = ir_analog_in_back.read() / 1000;     //define variable to help function

    //outputs
    //motors
    DigitalOut enable_motors_main(PA_10);
    DigitalOut enable_motors_spindle_front(PA_9);
    DigitalOut enable_motors_spindle_ba(PB_13);

    EncoderCounter  encoder_M1(PA_0, PA_1); // create encoder objects to read in the encoder counter values !pins fehlen!
    EncoderCounter  encoder_M2(PB_6, PB_7);
    EncoderCounter  encoder_M3(PA_6, PC_7);

    float   pwm_period_s = 0.00005f;    //PWM-Periode fürd DC-Motoren definiert

    FastPWM pwm_M1(PA_10);              
    FastPWM pwm_M2(PA_9);               
    FastPWM pwm_M3(PB_13);             

    // 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

    PositionController positionController_M1(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M1, encoder_M1);
    PositionController positionController_M2(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M2, encoder_M2);
    PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3);

    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

    //servo
    Servo servo_offshoot(PB_12);
    float servo_offshoot_angle = 0;
    int servo_period_mus = 20000;

    //Eingänge
    DigitalIn user_button(PC_13);

// 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

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

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

//functions
    //Space
    //inputs
bool ir_front(){                        //function to check if desired goal in height is reached
    if(ir_front_mV <= 830.0f){
        return true;
    }else{
        return false;
    }
}

bool ir_front_end(){
     if(ir_front_mV <= 000.0f){
        return true;
    }else{
        return false;
    }
}

bool ir_back(){                      //function to check if desired goal in height is reached
      if(ir_back_mV <= 000.0f){
        return true;
    }else{
        return false;
    }
}

bool end_step(){                        //function to check if next step is reached
    if(endswitch_step.read()){             //define pullup mode!!
    return true;
    }else{
        return false;
    }
}

bool end_spindel_f(){                   //function to check if front spindle is in position 0
    if(endswitch_spindle_f.read()){
        return true;
    }else{
        return false;
    }
}

bool end_spindel_b(){                   //function to check if back spindle is in position 0
    if(endswitch_spindle_b.read()){
        return true;
    }else{
        return false;
    }
}

    //movements
void drive(){                                                 //function to drive forward until next step is reached
    while(1){
        enable_motors_main = 1;
        positionController_M1.setDesiredRotation(1.5f, 0.2f);
        }
        enable_motors_main = 0;
}

void spindle_up_front(){                                            //function to move spindles up as soon as step is reached and until desired goal in height is hit
        while(true){
        enable_motors_spindle_front = 1;
        positionController_M2.setDesiredRotation(1.5f, 0.2f);
        }
        enable_motors_spindle_front = 0;
}

void spindle_up_back(){                                            //function to move spindles up as soon as step is reached and until desired goal in height is hit
        while(true){                                                 
        enable_motors_spindle_ba = 1;
        positionController_M2.setDesiredRotation(1.5f, 0.2f);
        }
        enable_motors_spindle_ba = 0;
}

void spindle_down_front(){                                          //function to move spindles down until endswitches of spindles show true
        while(true){                                                //reverse Motor drive direction
        enable_motors_spindle_front = 1;
        positionController_M3.setDesiredRotation(1.5f, 0.2f);
        }
        enable_motors_spindle_front = 0;
}

void spindle_down_back(){                                        //function to move spindles down until endswitches of spindles show true
    while(true){                                                 //reverse Motor drive direction
        enable_motors_spindle_ba = 1;
        positionController_M3.setDesiredRotation(1.5f, 0.2f);
        }
        enable_motors_spindle_ba = 0;
}

void offshoot_out(){
     while(true){
        if (!servo_offshoot.isEnabled()) servo_offshoot.Enable(servo_offshoot_angle, servo_period_mus);
        servo_offshoot_angle = 1;
        servo_offshoot.Enable(servo_offshoot_angle, servo_period_mus);
        servo_offshoot.SetPosition(servo_offshoot_angle);
    }

    if (servo_offshoot.isEnabled()) servo_offshoot.Disable();
}

void offshoot_in(){
    while(true){
        if (!servo_offshoot.isEnabled()) servo_offshoot.Enable(servo_offshoot_angle, servo_period_mus);
        servo_offshoot_angle = 0;
        servo_offshoot.Enable(servo_offshoot_angle, servo_period_mus);
        servo_offshoot.SetPosition(servo_offshoot_angle);
    }

    if (servo_offshoot.isEnabled()) servo_offshoot.Disable();
    
}

int main()
{
    endswitch_step.mode(PullUp);
    endswitch_spindle_f.mode(PullUp);
    endswitch_spindle_b.mode(PullUp);

    bool start, stopp, reset;
	int counter_start, counter_reset;
    int counter_user_button, counter_stoppen;
    bool drive_down;

while(1){
		
		//Counter für Start Stopp Reset
		if ((user_button.read() == 0) & (counter_stoppen == 0)) {
	        counter_user_button++;
	        counter_stoppen = 1;
	
	    }
		if (user_button.read()){
	        counter_stoppen = 0;
	    }
	    if ((counter_user_button >= 4) || (drive_down == 1)){
	        counter_user_button = 0;
	    }
    
	    switch(counter_user_button){
	    	case 0:
	    		start = 0;
	    		stopp = 0;
	    		reset = 0;
	    		drive_down = 0;
	    	break;
			case 1:
	    		start = 1;
	    		stopp = 0;
	    		reset = 0;
	    	break;
	    	case 2:
	    		start = 0;
	    		stopp = 1;
	    		reset = 0;
	    	break;
	    	case 3:
	    		start = 0;
	    		stopp = 0;
	    		reset = 1;
	    	break;
		}       
		
		//Startbedingungen für Schlaufen
		if(start == 1) counter_start = 1;
		if(reset == 1) counter_reset = 1;
		if(start == 0) counter_start = 0;
		if(reset == 0) counter_reset = 0;
    
	
		switch(counter_start){
			case 1: //Roboter nach vorne fahren
				drive() = 1;
				counter_start++;
			break;
			case 2: //Roboter stoppen wenn er bei der Treppe ankommt oder es keine weitere Stufe hat
					if (end_step() == 1){
						drive() = 0;
						counter_start++;
					}
					if (ir_front_end() == 1){
						drive() = 0;
						counter_start = 0;
						drive_down = 1; // Wenn nicht runtergefahren wird, kann man diese Variable zum rücksetzen des Startes verwenden
					}
			break;
			case 3: //Stelze vorne und hinten ausfahren
				spindle_up_front() = 1;
				spindle_up_back() = 1;
				counter_start++;
			break;
			case 4: //Stelzen anhalten wenn die Höhe erreicht ist
				if (ir_front() == 1){
					spindle_up_front() = 0;
				}
				if ( ir_back()== 1){
					spindle_up_back() = 0;
				}
				if (ir_back() && ir_front()){
					counter_start++;
				}
			break;
			case 5: //Ableger ausfahren
				offshoot_out() = 1;
				if (ableger_ausgefahren == 1) {
					offshoot_out() = 0;
					counter_start++;
				}
			break;
			case 6: //Stelzen vorne einfahren
				spindle_down_front() = 1;
				if (end_spindel_f() == 1){
					spindle_up_front() = 0;
					counter_start++;
				}
			break;
			case 7: //nach vorne fahren, damit der Ableger ganz auf der nächsten Stufe ist
				antriebsmotor_vor = 1;
				//noch nicht fertig
				//Muss Zeit- oder Positionsgesteuert werden
				counter_start++;
			break;
			case 8: //Stelzen hinten einfahren
				spindle_up_back() = 1;
				if (end_spindel_b() == 1){
					spindle_up_back() = 0;
					counter_start++;
				}
			break;
			case 9: //Ableger wieder einfahren
				offshoot_in() = 1;
				if (ableger_eingefahren == 1){
					offshoot_in() = 0;
					counter_start = 1;
				}
			break;

		}
	
	
		if (stopp == 1){  //Alle Aktionen werden gestoppt
			drive() = 0; 
			spindle_up_front() = 0;
			spindle_up_back() = 0;
			spindle_down_front() = 0;
			spindle_down_back() = 0;
			offshoot_out() = 0;
			offshoot_in() = 0;	
			drive_down = 0;
		}
	

		switch (counter_reset){
			case 1:
				if (ableger_eingefahren != 1) offshoot_in() = 1;
				if (ableger_eingefahren == 1){
				offshoot_in() = 0;
				counter_reset++;
				}
			break;
			case 2:
				if (end_spindel_f() != 1) spindle_up_front() = 1;
				if (end_spindel_f() == 1){
				spindle_up_front() = 0;
				counter_reset++;
				}
			break;
			case 3:
				if (end_spindel_b() != 1) spindle_up_back() = 1;
				if (end_spindel_b() == 1){
				spindle_up_back() = 0;
				counter_reset = 0;
				}
			break;
		}
					
		
	}
}