File content as of revision 38:cbad84e4c714:

/*this Program acts as a prototype. it dosn't include any PWM modulation nor cours-correcten.
It is built as Step after Step programm, if any step fails the Program feils it shoud proof the concept of the pland roboter
*/
#include "AnalogIn.h"
#include "DigitalIn.h"
#include "DigitalOut.h"
#include "PinNames.h"
#include "mbed.h"
#include "pinmap.h"
#include <cstdint>
#include <cwchar>
#include "PM2_Libary.h"

//------------------------------------general IO and Variables----------------------------------------------------------

Timer user_button_timer;            // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
void user_button_released_fcn();

int ToNextFunction = 0 ;
float trigerValueRearSensor = 0.0f;
float trigerValueFrontSensor = 0.0f;

//-----------------------------------------actors Definition----------------------------------------------------
// Infrarot sensors pin declaration
AnalogIn FrontSensor(PC_6); //IR TOF sensor at the front facing down
AnalogIn RearSensor(PC_8); //IR TOF sensor at the back facing down
AnalogIn LeftEyeSensor(PC_2); // Infrared sensor on top in "head" part left eye
AnalogIn RightEyeSensor(PC_3); // Infrared sensor on top in "head" part right eye

//Encoder pin declaration
EncoderCounter  encoder_M_right(PA_6, PC_7); //encoder pin decalaration for wheels right side
EncoderCounter  encoder_M_left(PB_6, PB_7); //encoder pin decalaration for wheels left side
EncoderCounter encoder_M_arm(PA_0, PA_1); //encoder pin decalaration for arm

//motor pin declaration
FastPWM pwm_M_right(PA_9);              
FastPWM pwm_M_left(PB_13);
FastPWM pwm_M_arm(PA_10);
// 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_wheels = 2000.0f * 100.0f;    // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
float counts_per_turn_arm = 40000.0f * 100.0f;      // define counts per turn at gearbox end (counts/turn * gearratio) for arm
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

//motors for tracks
PositionController positionController_M_right(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
PositionController positionController_M_left(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here

//Arm Motor
PositionController positionController_M_Arm(counts_per_turn_arm * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_arm, encoder_M_arm); // parameters adjusted to 100:1 gear, we need a different speed controller gain here


//Platzhalter Variabeln für die Positionierung
int16_t PositionStair    = 20;
int16_t PositionBackOff  = 100;
int16_t degArmStart      = 40;
int16_t degArmLift       = 18;



//-----------------------------------------Functions----------------------------------------------------------
//only moves the arm in to the starting position
int StartPosition(void){
    if(true){
        return 1;
    }
    return NULL;
}

//Drives forward into the next step
int Drive(int16_t dist){
    int8_t i = 0;         //prov condition variable


    int8_t distance = dist; //distance which will be driven in [mm]
    float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
    float distanceValue = float(distance)*factor;

        positionController_M_right.setDesiredRotation(distanceValue);
        positionController_M_left.setDesiredRotation(distanceValue);

    return 0;
    
}

//only turns the arm until the robot is on the next step
//not yet clear if the motor controler function drives to a absolute poition or if it drives the given distance relative to the current position
int LiftUp(int16_t deg){
    int8_t rotation = deg;
    int8_t i = 0;         //prov condition variable
    do{
        positionController_M_Arm.setDesiredRotation(deg);
        
        i++;
    }while(i < 1);
    return 0;
}



    
int main()
{
    // attach button fall and rise functions to user button object
    user_button.fall(&user_button_pressed_fcn);
    user_button.rise(&user_button_released_fcn);


    while (true) {


       
        switch (ToNextFunction) {
        case 1: StartPosition();
                ToNextFunction+=1;
        break;
        case 2: Drive(PositionStair);
                ToNextFunction+=1;
        break;
        case 3: LiftUp(degArmLift);
                ToNextFunction+=1;
        break;
        case 4: Drive(PositionBackOff);
                ToNextFunction+=1;
        break;
        case 5: LiftUp(degArmStart);
                ToNextFunction = 0;
        break;
        default: ToNextFunction = 0;     
        }

        
        thread_sleep_for(10);
    }

}
void user_button_pressed_fcn()
{
    user_button_timer.start();
    user_button_timer.reset();
}

void user_button_released_fcn()
{
    // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
    int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
    user_button_timer.stop();
    if (user_button_elapsed_time_ms > 200) {
        ToNextFunction = 1;
    }




}