Benjamin Fietzke
Summerschool m3pi MRT
Revision 0:d1ca55d6ef68, committed 2019-11-14
- Comitter:
- Benf
- Date:
- Thu Nov 14 09:35:14 2019 +0000
- Commit message:
- Summerschool m3pi Project MRT
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi_ng.lib Thu Nov 14 09:35:14 2019 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/ngoldin/code/m3pi_ng/#5589eef1f879
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Nov 14 09:35:14 2019 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h" // Mbed Library
+#include "m3pi_ng.h" // Nico's Library
+#include "btbee.h" // Bluetooth Library
+
+m3pi Rob; // create m3pi Object
+DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)}; // LEDs of 3PI
+DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)}; // LEDs of M3PI
+
+DigitalIn m3pi_pb(p21,PullUp); // DigitalIn: User Button P21
+DigitalIn m3pi_fir(p12); // DigitalIn: Front IR-Senspor Pin: 12
+
+Timer timer;
+
+////////////////////// Variables and Parameters ///////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+int sen_raw[5], sen_cal[5], sen_norm[5];
+int n_black = 0;
+int lap = 0;
+
+float e, u, vleft, vright;
+float dt=0.005;
+float t=0, t0=0, t_lap0=0, t_lap=0, t0_code=0, t_code=.1, t_code_max=0;
+///////////////////////////// Functions ///////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+/////// Function for Initilization the Robot Program ///////
+////////////////////////////////////////////////////////////
+void Init()
+ {
+ Rob.locate(0,0); Rob.printf("batteries:");
+ Rob.locate(0,1); Rob.printf("%0.2f V",Rob.battery());
+ wait(1);
+ Rob.cls();
+ Rob.sensor_auto_calibrate();
+
+ for (int i = 0 ; i<8 ; i++)
+ { m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }
+
+ }
+////////// Function for Ending the Robot Program //////////
+////////////////////////////////////////////////////////////
+void End(float Data, int rows, int columns)
+ {
+ Rob.locate(0,0); Rob.printf("Process");
+ Rob.locate(0,1); Rob.printf("done.");
+ Rob.stop();
+ }
+////////// Function for Motor Voltage Saturation //////////
+////////////////////////////////////////////////////////////
+void sat(float &v_sat)
+ {
+ if (v_sat > 1)
+ v_sat=1;
+ if (v_sat < -1)
+ v_sat=-1;
+ }
+////////// Function for Automatic Line Following ///////////
+////////////////////////////////////////////////////////////
+void Linefollowing()
+ {
+ const double Kp = 0.60; // [default: 0.8]
+ float vforw = Kp * 0.5;
+
+ e = Rob.line_position();
+ u = Kp*e;
+ vleft = vforw + u;
+ vright = vforw - u;
+ sat(vleft); sat(vright); // actuator saturation
+
+ Rob.left_motor(vleft); // push to 3pi --> AC-Motor
+ Rob.right_motor(vright); // push to 3pi --> AC-Motor
+ }
+/////////// Function for Avoiding a FrontCrash /////////////
+////////////////////////////////////////////////////////////
+void Anticrash()
+ {
+ while (!m3pi_fir)
+ {
+ Rob.stop();
+
+ for (int i = 0 ; i<8 ; i++)
+ { m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }
+
+ }
+
+ }
+
+/////////// Function for lap time & lap counter ////////////
+////////////////////////////////////////////////////////////
+void Counter()
+ {
+ Rob.calibrated_sensor(sen_cal);
+
+ for (int i = 0 ; i<5 ; i++)
+ {sen_norm[i] = sen_cal[i]/1000;}
+
+ if ( (sen_norm[0]>=.8) && (sen_norm[2]>=.8) && (sen_norm[4]>=.8) )
+ {
+ n_black++;
+ t = timer.read();
+
+ if ( (n_black == 5)&&((t-t_lap0)>2) )
+ {
+ lap++;
+ t_lap = t-t_lap0;
+ t_lap0 = t;
+
+ Rob.cls();
+ Rob.locate(0,0);
+ Rob.printf("lap: %i",lap);
+ Rob.locate(0,1);
+ Rob.printf("t: %4.2f",t_lap);
+
+ }
+
+ }
+ else
+ {
+ n_black=0;
+
+ }
+ }
+
+/////////////////////////////// MAIN /////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+int main()
+{
+////////////// Initialization ///////////////
+////////////////////////////////////////////////////////////
+Init();
+////////////////////////////////////////////////////////////
+
+float tcmp = 1;
+
+timer.start();
+ while (true)
+ {
+// Anticrash();
+ t = timer.read();
+
+ if ( (t-t0) >= dt)
+ {
+ t0 = t;
+ t0_code = t;
+
+
+ Linefollowing();
+ Counter();
+
+ t_code = (timer.read()-t0_code)*1000;
+ tcmp = t_code/t_code_max;
+
+ if ( tcmp> 1.5)
+ {t_code_max = t_code;
+ Rob.cls(); Rob.locate(0,1); Rob.printf("%3.2f ms",t_code_max);
+ }
+ }
+ }
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 14 09:35:14 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file