Felix W / Mbed OS PM2_Example_PES_board_waldsfel

Workshop2

Dependencies:   PM2_Libary

main.cpp@12:e5be1469bc76, 2021-04-14 (annotated)

Committer:
waldsfel
Date:
Wed Apr 14 14:08:47 2021 +0000
Revision:
12:e5be1469bc76
Parent:
11:af0f165f8761
Child:
13:bc1df4256578

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


pmic

1:93d997d6b232


     1

#include "mbed.h"





pmic

1:93d997d6b232


     2

#include "platform/mbed_thread.h"





pmic

6:e1fa1a2d7483


     3







pmic

6:e1fa1a2d7483


     4

/* PM2_Libary */





pmic

1:93d997d6b232


     5

#include "EncoderCounter.h"





pmic

1:93d997d6b232


     6

#include "Servo.h"





pmic

3:d22942631cd7


     7

#include "SpeedController.h"





pmic

9:f10b974d01e0


     8

#include "FastPWM.h"





pmic

11:af0f165f8761


     9

#include "RangeFinder.h"





pmic

6:e1fa1a2d7483


    10







pmic

6:e1fa1a2d7483


    11

using namespace std::chrono;





pmic

6:e1fa1a2d7483


    12







pmic

8:9bb806a7f585


    13

InterruptIn user_button(USER_BUTTON);





pmic

8:9bb806a7f585


    14

DigitalOut  led(LED1);





pmic

6:e1fa1a2d7483


    15







pmic

6:e1fa1a2d7483


    16

bool  executeMainTask = false;





pmic

6:e1fa1a2d7483


    17

Timer user_button_timer, loop_timer;





pmic

7:c0f5bb355f41


    18

int   Ts_ms = 50;





pmic

6:e1fa1a2d7483


    19







pmic

6:e1fa1a2d7483


    20

/* declaration of custom button functions */





pmic

6:e1fa1a2d7483


    21

void button_fall();





pmic

6:e1fa1a2d7483


    22

void button_rise();





pmic

6:e1fa1a2d7483


    23







pmic

11:af0f165f8761


    24

// SHARP GP2Y0A21Y IR Sensor





pmic

11:af0f165f8761


    25

// https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds





pmic

6:e1fa1a2d7483


    26

/* create analog input object */





pmic

6:e1fa1a2d7483


    27

AnalogIn analogIn(PC_2);





pmic

11:af0f165f8761


    28

float    dist_IRSensor = 0.0f;





pmic

6:e1fa1a2d7483


    29







pmic

11:af0f165f8761


    30

// 78:1 Metal Gearmotor 20Dx43L mm 12V CB





pmic

11:af0f165f8761


    31

// https://www.pololu.com/product/3477





pmic

6:e1fa1a2d7483


    32

/* create enable dc motor digital out object */





pmic

6:e1fa1a2d7483


    33

DigitalOut enable_motors(PB_15);





pmic

10:c5d85e35758c


    34

/* create pwm objects */





pmic

10:c5d85e35758c


    35

FastPWM pwmOut_M1(PB_13);





pmic

10:c5d85e35758c


    36

FastPWM pwmOut_M2(PA_9);





pmic

10:c5d85e35758c


    37

FastPWM pwmOut_M3(PA_10);





pmic

10:c5d85e35758c


    38

double  Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)





pmic

6:e1fa1a2d7483


    39

/* create encoder read objects */





pmic

10:c5d85e35758c


    40

EncoderCounter  encoderCounter_M1(PA_6, PC_7);





pmic

10:c5d85e35758c


    41

EncoderCounter  encoderCounter_M2(PB_6, PB_7);





pmic

10:c5d85e35758c


    42

EncoderCounter  encoderCounter_M3(PA_0, PA_1);





pmic

10:c5d85e35758c


    43

/* create speed controller objects, only M1 and M2, M3 is used open-loop */





pmic

10:c5d85e35758c


    44

float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio





pmic

10:c5d85e35758c


    45

float kn = 180.0f/12.0f;               // (RPM/V)





pmic

10:c5d85e35758c


    46

float max_voltage = 12.0f;             // adjust this to 6.0f if only one batterypack is used





pmic

10:c5d85e35758c


    47

SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);





pmic

10:c5d85e35758c


    48

SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);





pmic

6:e1fa1a2d7483


    49







pmic

11:af0f165f8761


    50

// Futaba Servo S3001 20mm 3kg Analog 





pmic

11:af0f165f8761


    51

// https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE





pmic

6:e1fa1a2d7483


    52

/* create servo objects */





pmic

10:c5d85e35758c


    53

Servo servo_S1(PB_2);





pmic

10:c5d85e35758c


    54

Servo servo_S2(PC_8);





waldsfel

12:e5be1469bc76


    55

Servo servo_S3(PC_6);





pmic

11:af0f165f8761


    56

// Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below





pmic

10:c5d85e35758c


    57

int servoPeriod_mus = 20000;





pmic

10:c5d85e35758c


    58

int servoOutput_mus_S1 = 0;





pmic

10:c5d85e35758c


    59

int servoOutput_mus_S2 = 0;





waldsfel

12:e5be1469bc76


    60

int servoOutput_mus_S3 = 0;





pmic

10:c5d85e35758c


    61

int servo_counter = 0;





pmic

10:c5d85e35758c


    62

int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));





pmic

1:93d997d6b232


    63







pmic

11:af0f165f8761


    64

// Groove Ultrasonic Ranger V2.0





pmic

11:af0f165f8761


    65

// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 





pmic

11:af0f165f8761


    66

/* create range finder object (ultra sonic distance sensor) */





pmic

11:af0f165f8761


    67

RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization





pmic

11:af0f165f8761


    68

// RangeFinder rangefinder(PB_6, 5782.0f, 0.02f,  7000); // 1/Ts_ms = 50 Hz parametrization





pmic

11:af0f165f8761


    69

float       dist_USSensor = 0.0f;





pmic

11:af0f165f8761


    70







pmic

1:93d997d6b232


    71

int main()





pmic

9:f10b974d01e0


    72

{





pmic

6:e1fa1a2d7483


    73

    user_button.fall(&button_fall);





pmic

6:e1fa1a2d7483


    74

    user_button.rise(&button_rise);





pmic

6:e1fa1a2d7483


    75

    loop_timer.start();





pmic

6:e1fa1a2d7483


    76







pmic

10:c5d85e35758c


    77

    /* enable hardwaredriver dc motors */





pmic

10:c5d85e35758c


    78

    enable_motors = 1;





pmic

10:c5d85e35758c


    79

    /* initialize pwm for motor M3*/





pmic

10:c5d85e35758c


    80

    pwmOut_M3.period(Ts_pwm_s);





pmic

6:e1fa1a2d7483


    81

    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */





pmic

10:c5d85e35758c


    82

    pwmOut_M3.write(0.5);





pmic

9:f10b974d01e0


    83







pmic

10:c5d85e35758c


    84

    /* enable servos, you can also disable them */





pmic

10:c5d85e35758c


    85

    servo_S1.Enable(servoOutput_mus_S1, servoPeriod_mus);





pmic

10:c5d85e35758c


    86

    servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);





waldsfel

12:e5be1469bc76


    87

    servo_S3.Enable(servoOutput_mus_S3, servoPeriod_mus);





pmic

6:e1fa1a2d7483


    88







pmic

1:93d997d6b232


    89

    while (true) {





pmic

6:e1fa1a2d7483


    90







pmic

6:e1fa1a2d7483


    91

        loop_timer.reset();





pmic

6:e1fa1a2d7483


    92







pmic

6:e1fa1a2d7483


    93

        /* ------------- start hacking ------------- -------------*/





pmic

6:e1fa1a2d7483


    94







pmic

6:e1fa1a2d7483


    95

        if (executeMainTask) {





pmic

6:e1fa1a2d7483


    96







pmic

6:e1fa1a2d7483


    97

            /* read analog input */





pmic

11:af0f165f8761


    98

            dist_IRSensor = analogIn.read() * 3.3f;





pmic

6:e1fa1a2d7483


    99







pmic

10:c5d85e35758c


   100

            /* command a speed to dc motors M1 and M2*/





pmic

10:c5d85e35758c


   101

            speedController_M1.setDesiredSpeedRPS( 1.0f);





pmic

10:c5d85e35758c


   102

            speedController_M2.setDesiredSpeedRPS(-0.5f);





pmic

10:c5d85e35758c


   103

            /* write output voltage to motor M3 */





pmic

10:c5d85e35758c


   104

            pwmOut_M3.write(0.75);





pmic

6:e1fa1a2d7483


   105







pmic

10:c5d85e35758c


   106

            /* command servo position via output time, this needs to be calibrated */





pmic

10:c5d85e35758c


   107

            servo_S1.SetPosition(servoOutput_mus_S1);





pmic

10:c5d85e35758c


   108

            servo_S2.SetPosition(servoOutput_mus_S2);





waldsfel

12:e5be1469bc76


   109

            servo_S3.SetPosition(servoOutput_mus_S3);





pmic

10:c5d85e35758c


   110

            if (servoOutput_mus_S1 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {





pmic

10:c5d85e35758c


   111

                servoOutput_mus_S1 += 100;





pmic

8:9bb806a7f585


   112

            }





pmic

10:c5d85e35758c


   113

            if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {





pmic

10:c5d85e35758c


   114

                servoOutput_mus_S2 += 100;





pmic

8:9bb806a7f585


   115

            }





waldsfel

12:e5be1469bc76


   116

            if (servoOutput_mus_S3 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {





waldsfel

12:e5be1469bc76


   117

                servoOutput_mus_S3 += 100;





waldsfel

12:e5be1469bc76


   118

            }





pmic

10:c5d85e35758c


   119

            servo_counter++;





pmic

6:e1fa1a2d7483


   120







pmic

11:af0f165f8761


   121

            /* read ultra sonic distance sensor */





pmic

11:af0f165f8761


   122

            dist_USSensor = rangeFinder.read_cm();





pmic

11:af0f165f8761


   123







pmic

6:e1fa1a2d7483


   124

            /* visual feedback that the main task is executed */





pmic

6:e1fa1a2d7483


   125

            led = !led;





pmic

9:f10b974d01e0


   126







pmic

1:93d997d6b232


   127

        } else {





pmic

6:e1fa1a2d7483


   128







pmic

11:af0f165f8761


   129

            dist_IRSensor = 0.0f;





pmic

1:93d997d6b232


   130







pmic

10:c5d85e35758c


   131

            speedController_M1.setDesiredSpeedRPS(0.0f);





pmic

10:c5d85e35758c


   132

            speedController_M2.setDesiredSpeedRPS(0.0f);





pmic

10:c5d85e35758c


   133

            pwmOut_M3.write(0.5);





pmic

6:e1fa1a2d7483


   134







pmic

10:c5d85e35758c


   135

            servoOutput_mus_S1 = 0;





pmic

10:c5d85e35758c


   136

            servoOutput_mus_S2 = 0;





waldsfel

12:e5be1469bc76


   137

            servoOutput_mus_S3 = 0;





pmic

10:c5d85e35758c


   138

            servo_S1.SetPosition(servoOutput_mus_S1);





pmic

10:c5d85e35758c


   139

            servo_S2.SetPosition(servoOutput_mus_S2);





waldsfel

12:e5be1469bc76


   140

            servo_S3.SetPosition(servoOutput_mus_S3);





waldsfel

12:e5be1469bc76


   141







pmic

11:af0f165f8761


   142

            dist_USSensor = 0.0f;





pmic

6:e1fa1a2d7483


   143







pmic

6:e1fa1a2d7483


   144

            led = 0;





pmic

1:93d997d6b232


   145

        }





pmic

6:e1fa1a2d7483


   146







pmic

10:c5d85e35758c


   147

        /* do only output via serial what's really necessary (this makes your code slow)*/





waldsfel

12:e5be1469bc76


   148

        printf("%3.3f, %3d, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",





pmic

11:af0f165f8761


   149

               dist_IRSensor,





pmic

10:c5d85e35758c


   150

               servoOutput_mus_S1,





pmic

10:c5d85e35758c


   151

               servoOutput_mus_S2,





waldsfel

12:e5be1469bc76


   152

               servoOutput_mus_S3,





pmic

10:c5d85e35758c


   153

               encoderCounter_M3.read(),





pmic

10:c5d85e35758c


   154

               speedController_M1.getSpeedRPS(),





pmic

11:af0f165f8761


   155

               speedController_M2.getSpeedRPS(),





pmic

11:af0f165f8761


   156

               dist_USSensor);





pmic

8:9bb806a7f585


   157







pmic

6:e1fa1a2d7483


   158

        /* ------------- stop hacking ------------- -------------*/





pmic

6:e1fa1a2d7483


   159







pmic

6:e1fa1a2d7483


   160

        int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();





pmic

6:e1fa1a2d7483


   161

        int dT_loop_ms = Ts_ms - T_loop_ms;





pmic

6:e1fa1a2d7483


   162

        thread_sleep_for(dT_loop_ms);





pmic

1:93d997d6b232


   163

    }





pmic

1:93d997d6b232


   164

}





pmic

6:e1fa1a2d7483


   165







pmic

6:e1fa1a2d7483


   166

void button_fall()





pmic

6:e1fa1a2d7483


   167

{





pmic

6:e1fa1a2d7483


   168

    user_button_timer.reset();





pmic

6:e1fa1a2d7483


   169

    user_button_timer.start();





pmic

6:e1fa1a2d7483


   170

}





pmic

6:e1fa1a2d7483


   171







pmic

6:e1fa1a2d7483


   172

void button_rise()





pmic

6:e1fa1a2d7483


   173

{





pmic

6:e1fa1a2d7483


   174

    int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();





pmic

6:e1fa1a2d7483


   175

    user_button_timer.stop();





pmic

8:9bb806a7f585


   176

    if (t_button_ms > 200) {





pmic

6:e1fa1a2d7483


   177

        executeMainTask = !executeMainTask;





pmic

8:9bb806a7f585


   178

    }





pmic

6:e1fa1a2d7483


   179

}