Benjamin Fietzke / Mbed 2 deprecated Autodrive_fixed_Stepsize

Summerschool m3pi MRT

Dependencies:   mbed m3pi_ng

main.cpp@0:d1ca55d6ef68, 2019-11-14 (annotated)

Committer:
Benf
Date:
Thu Nov 14 09:35:14 2019 +0000
Revision:
0:d1ca55d6ef68
Summerschool m3pi Project MRT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Benf 0:d1ca55d6ef68 1 #include "mbed.h" // Mbed Library
Benf 0:d1ca55d6ef68 2 #include "m3pi_ng.h" // Nico's Library
Benf 0:d1ca55d6ef68 3 #include "btbee.h" // Bluetooth Library
Benf 0:d1ca55d6ef68 4
Benf 0:d1ca55d6ef68 5 m3pi Rob; // create m3pi Object
Benf 0:d1ca55d6ef68 6 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)}; // LEDs of 3PI
Benf 0:d1ca55d6ef68 7 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)}; // LEDs of M3PI
Benf 0:d1ca55d6ef68 8
Benf 0:d1ca55d6ef68 9 DigitalIn m3pi_pb(p21,PullUp); // DigitalIn: User Button P21
Benf 0:d1ca55d6ef68 10 DigitalIn m3pi_fir(p12); // DigitalIn: Front IR-Senspor Pin: 12
Benf 0:d1ca55d6ef68 11
Benf 0:d1ca55d6ef68 12 Timer timer;
Benf 0:d1ca55d6ef68 13
Benf 0:d1ca55d6ef68 14 ////////////////////// Variables and Parameters ///////////////////////////
Benf 0:d1ca55d6ef68 15 /////////////////////////////////////////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 16 int sen_raw[5], sen_cal[5], sen_norm[5];
Benf 0:d1ca55d6ef68 17 int n_black = 0;
Benf 0:d1ca55d6ef68 18 int lap = 0;
Benf 0:d1ca55d6ef68 19
Benf 0:d1ca55d6ef68 20 float e, u, vleft, vright;
Benf 0:d1ca55d6ef68 21 float dt=0.005;
Benf 0:d1ca55d6ef68 22 float t=0, t0=0, t_lap0=0, t_lap=0, t0_code=0, t_code=.1, t_code_max=0;
Benf 0:d1ca55d6ef68 23 ///////////////////////////// Functions ///////////////////////////////////
Benf 0:d1ca55d6ef68 24 /////////////////////////////////////////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 25
Benf 0:d1ca55d6ef68 26 /////// Function for Initilization the Robot Program ///////
Benf 0:d1ca55d6ef68 27 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 28 void Init()
Benf 0:d1ca55d6ef68 29 {
Benf 0:d1ca55d6ef68 30 Rob.locate(0,0); Rob.printf("batteries:");
Benf 0:d1ca55d6ef68 31 Rob.locate(0,1); Rob.printf("%0.2f V",Rob.battery());
Benf 0:d1ca55d6ef68 32 wait(1);
Benf 0:d1ca55d6ef68 33 Rob.cls();
Benf 0:d1ca55d6ef68 34 Rob.sensor_auto_calibrate();
Benf 0:d1ca55d6ef68 35
Benf 0:d1ca55d6ef68 36 for (int i = 0 ; i<8 ; i++)
Benf 0:d1ca55d6ef68 37 { m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }
Benf 0:d1ca55d6ef68 38
Benf 0:d1ca55d6ef68 39 }
Benf 0:d1ca55d6ef68 40 ////////// Function for Ending the Robot Program //////////
Benf 0:d1ca55d6ef68 41 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 42 void End(float Data, int rows, int columns)
Benf 0:d1ca55d6ef68 43 {
Benf 0:d1ca55d6ef68 44 Rob.locate(0,0); Rob.printf("Process");
Benf 0:d1ca55d6ef68 45 Rob.locate(0,1); Rob.printf("done.");
Benf 0:d1ca55d6ef68 46 Rob.stop();
Benf 0:d1ca55d6ef68 47 }
Benf 0:d1ca55d6ef68 48 ////////// Function for Motor Voltage Saturation //////////
Benf 0:d1ca55d6ef68 49 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 50 void sat(float &v_sat)
Benf 0:d1ca55d6ef68 51 {
Benf 0:d1ca55d6ef68 52 if (v_sat > 1)
Benf 0:d1ca55d6ef68 53 v_sat=1;
Benf 0:d1ca55d6ef68 54 if (v_sat < -1)
Benf 0:d1ca55d6ef68 55 v_sat=-1;
Benf 0:d1ca55d6ef68 56 }
Benf 0:d1ca55d6ef68 57 ////////// Function for Automatic Line Following ///////////
Benf 0:d1ca55d6ef68 58 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 59 void Linefollowing()
Benf 0:d1ca55d6ef68 60 {
Benf 0:d1ca55d6ef68 61 const double Kp = 0.60; // [default: 0.8]
Benf 0:d1ca55d6ef68 62 float vforw = Kp * 0.5;
Benf 0:d1ca55d6ef68 63
Benf 0:d1ca55d6ef68 64 e = Rob.line_position();
Benf 0:d1ca55d6ef68 65 u = Kp*e;
Benf 0:d1ca55d6ef68 66 vleft = vforw + u;
Benf 0:d1ca55d6ef68 67 vright = vforw - u;
Benf 0:d1ca55d6ef68 68 sat(vleft); sat(vright); // actuator saturation
Benf 0:d1ca55d6ef68 69
Benf 0:d1ca55d6ef68 70 Rob.left_motor(vleft); // push to 3pi --> AC-Motor
Benf 0:d1ca55d6ef68 71 Rob.right_motor(vright); // push to 3pi --> AC-Motor
Benf 0:d1ca55d6ef68 72 }
Benf 0:d1ca55d6ef68 73 /////////// Function for Avoiding a FrontCrash /////////////
Benf 0:d1ca55d6ef68 74 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 75 void Anticrash()
Benf 0:d1ca55d6ef68 76 {
Benf 0:d1ca55d6ef68 77 while (!m3pi_fir)
Benf 0:d1ca55d6ef68 78 {
Benf 0:d1ca55d6ef68 79 Rob.stop();
Benf 0:d1ca55d6ef68 80
Benf 0:d1ca55d6ef68 81 for (int i = 0 ; i<8 ; i++)
Benf 0:d1ca55d6ef68 82 { m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }
Benf 0:d1ca55d6ef68 83
Benf 0:d1ca55d6ef68 84 }
Benf 0:d1ca55d6ef68 85
Benf 0:d1ca55d6ef68 86 }
Benf 0:d1ca55d6ef68 87
Benf 0:d1ca55d6ef68 88 /////////// Function for lap time & lap counter ////////////
Benf 0:d1ca55d6ef68 89 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 90 void Counter()
Benf 0:d1ca55d6ef68 91 {
Benf 0:d1ca55d6ef68 92 Rob.calibrated_sensor(sen_cal);
Benf 0:d1ca55d6ef68 93
Benf 0:d1ca55d6ef68 94 for (int i = 0 ; i<5 ; i++)
Benf 0:d1ca55d6ef68 95 {sen_norm[i] = sen_cal[i]/1000;}
Benf 0:d1ca55d6ef68 96
Benf 0:d1ca55d6ef68 97 if ( (sen_norm[0]>=.8) && (sen_norm[2]>=.8) && (sen_norm[4]>=.8) )
Benf 0:d1ca55d6ef68 98 {
Benf 0:d1ca55d6ef68 99 n_black++;
Benf 0:d1ca55d6ef68 100 t = timer.read();
Benf 0:d1ca55d6ef68 101
Benf 0:d1ca55d6ef68 102 if ( (n_black == 5)&&((t-t_lap0)>2) )
Benf 0:d1ca55d6ef68 103 {
Benf 0:d1ca55d6ef68 104 lap++;
Benf 0:d1ca55d6ef68 105 t_lap = t-t_lap0;
Benf 0:d1ca55d6ef68 106 t_lap0 = t;
Benf 0:d1ca55d6ef68 107
Benf 0:d1ca55d6ef68 108 Rob.cls();
Benf 0:d1ca55d6ef68 109 Rob.locate(0,0);
Benf 0:d1ca55d6ef68 110 Rob.printf("lap: %i",lap);
Benf 0:d1ca55d6ef68 111 Rob.locate(0,1);
Benf 0:d1ca55d6ef68 112 Rob.printf("t: %4.2f",t_lap);
Benf 0:d1ca55d6ef68 113
Benf 0:d1ca55d6ef68 114 }
Benf 0:d1ca55d6ef68 115
Benf 0:d1ca55d6ef68 116 }
Benf 0:d1ca55d6ef68 117 else
Benf 0:d1ca55d6ef68 118 {
Benf 0:d1ca55d6ef68 119 n_black=0;
Benf 0:d1ca55d6ef68 120
Benf 0:d1ca55d6ef68 121 }
Benf 0:d1ca55d6ef68 122 }
Benf 0:d1ca55d6ef68 123
Benf 0:d1ca55d6ef68 124 /////////////////////////////// MAIN /////////////////////////////////
Benf 0:d1ca55d6ef68 125 /////////////////////////////////////////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 126 int main()
Benf 0:d1ca55d6ef68 127 {
Benf 0:d1ca55d6ef68 128 ////////////// Initialization ///////////////
Benf 0:d1ca55d6ef68 129 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 130 Init();
Benf 0:d1ca55d6ef68 131 ////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 132
Benf 0:d1ca55d6ef68 133 float tcmp = 1;
Benf 0:d1ca55d6ef68 134
Benf 0:d1ca55d6ef68 135 timer.start();
Benf 0:d1ca55d6ef68 136 while (true)
Benf 0:d1ca55d6ef68 137 {
Benf 0:d1ca55d6ef68 138 // Anticrash();
Benf 0:d1ca55d6ef68 139 t = timer.read();
Benf 0:d1ca55d6ef68 140
Benf 0:d1ca55d6ef68 141 if ( (t-t0) >= dt)
Benf 0:d1ca55d6ef68 142 {
Benf 0:d1ca55d6ef68 143 t0 = t;
Benf 0:d1ca55d6ef68 144 t0_code = t;
Benf 0:d1ca55d6ef68 145
Benf 0:d1ca55d6ef68 146
Benf 0:d1ca55d6ef68 147 Linefollowing();
Benf 0:d1ca55d6ef68 148 Counter();
Benf 0:d1ca55d6ef68 149
Benf 0:d1ca55d6ef68 150 t_code = (timer.read()-t0_code)*1000;
Benf 0:d1ca55d6ef68 151 tcmp = t_code/t_code_max;
Benf 0:d1ca55d6ef68 152
Benf 0:d1ca55d6ef68 153 if ( tcmp> 1.5)
Benf 0:d1ca55d6ef68 154 {t_code_max = t_code;
Benf 0:d1ca55d6ef68 155 Rob.cls(); Rob.locate(0,1); Rob.printf("%3.2f ms",t_code_max);
Benf 0:d1ca55d6ef68 156 }
Benf 0:d1ca55d6ef68 157 }
Benf 0:d1ca55d6ef68 158 }
Benf 0:d1ca55d6ef68 159
Benf 0:d1ca55d6ef68 160 /////////////////////////////////////////////////////////////////////////////////////////////////
Benf 0:d1ca55d6ef68 161 }