Mechatro5
/
LINE_TRACE_CAR
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BMX055 Motor Way
Diff: main.cpp
- Revision:
- 6:c024519cce11
- Parent:
- 5:17314fc7b175
- Child:
- 7:ce2920da874b
--- a/main.cpp Mon Jan 13 07:07:47 2020 +0000
+++ b/main.cpp Wed Jan 15 02:18:48 2020 +0000
@@ -2,36 +2,33 @@
#include "mbed.h"
#include "motor.h"
#include "BMX055.h"
+#include "way.h"
-BMX055 bmx(D0, D1);
-
-DigitalIn button(D2);
+BMX055 bmx(D0, D1);
+DigitalIn button(D2);
DigitalOut motor_mode(D9);
DigitalOut LED(D10);
-Motor motorL(D3, D4);
-Motor motorR(D5, D6);
-InterruptIn pg1(D11);
-InterruptIn pg2(D12);
+Motor motorL(D5, D6);
+Motor motorR(D3, D4);
+InterruptIn pg1(D12);
+InterruptIn pg2(D11);
AnalogIn reflectorFL(A6);
AnalogIn reflectorFM(A3);
AnalogIn reflectorFR(A2);
AnalogIn reflectorBL(A1);
AnalogIn reflectorBR(A0);
-//AnalogIn reflectorF(A2);
-//AnalogIn reflectorL(A1);
-//AnalogIn reflectorR(A0);
-
Thread thread_trace;
Thread thread_motor;
-
Thread sensor_thread;
-const int THREAD_FLAG_TRACE(1);
-const int THREAD_FLAG_MOTOR(2);
-const float INTVAL_REFLECTOR(0.01);
+const float INTVAL_REFLECTOR(0.1);//0.01);
const float INTVAL_MOTOR(0.25);
+const float pi = 3.14159265359;
+const float gear_ratio = 1 / 38.2;
+const float tire_size = 0.57 / 2;
+
float dist = 0.0;
float x = 0.0;
float y = 0.0;
@@ -57,8 +54,9 @@
//const float standard_rpm = 0;
//const float slow_rpm = 0;
static float gap_pre = 0.0;
- const float KP = 5000;//850;
+ const float KP = 850;
const float KD = 3000;
+ const float K = 0.2;
const int L = 0;
const int R = 1;
@@ -67,123 +65,96 @@
motorL.Set_phase(L);
motorR.Set_phase(R);
-
-
float target_azimuth = 0.0;
float current_azimuth = 0.0;
float curvature = 0.0;
-
float tread = 0.097;
-
-
- while (true)
+ while (1)
{
- current_azimuth = bmx.get_azimuth_machineframe();
-
- if(current_azimuth>3.1416)
+ /*
+ if (reflectorFL>0.15)
{
- current_azimuth-=6.2832;
+ flag=L;
+ }
+ if (reflectorFR>0.15)
+ {
+ flag=R;
}
- curvature = 0.01 / (target_azimuth - current_azimuth);
-
- //motorL.Set_target(3000 - (1 - tread/curvature));
- //motorR.Set_target(3000 - (1 + tread/curvature));
+ if ( reflectorFM<=0.4 && reflectorFL<=0.15 && reflectorFR<=0.15 )
+ {
+ if (flag==L)
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(0);
+ motorR.Set_phase(R);
+ motorR.Set_target(slow_rpm);
+ printf("turnL\n");
+ }
+ else
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(slow_rpm);
+ motorR.Set_phase(R);
+ motorR.Set_target(0);
+ printf("turnR\n");
+ }
+ }
+ else
+ {
+ if ( reflectorFM>0.4 )
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(fast_rpm);
+ motorR.Set_phase(R);
+ motorR.Set_target(fast_rpm);
+ }
+ else
+ {
+ if ( reflectorBL>0.15 )
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(standard_rpm - standard_rpm*0.18);
+ motorR.Set_phase(R);
+ motorR.Set_target(standard_rpm + standard_rpm*0.18);
+ }
+ else
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(standard_rpm + standard_rpm*0.18);
+ motorR.Set_phase(R);
+ motorR.Set_target(standard_rpm - standard_rpm*0.18);
+ }
+ }
+ }*/
- motorL.Set_target(3000.0);
- motorR.Set_target(3000.0);
- }
-
- /*while (1)
- {
float gap_min = 0.0;
int i_min = 0;
- for (int i = 0; i < 500; i++)///////////////////n; i++)
+ for (int i = 0; i < 478; i++)
{
- float dist_gap = dist - way[i][3];
+ float dist_gap = dist - way[i][2];
if (dist_gap < gap_min)
{
gap_min = dist_gap;
i_min = i;
}
}
-
-
-
- if (reflectorFL > 0.1)
- {
- flag = L;
- }
- if (reflectorFR > 0.1)
- {
- flag = R;
- }
- if (reflectorFM <= 0.4 && reflectorFL <= 0.1 && reflectorFR <= 0.1)
- if (true)
- {
- if (flag == L)
- if (true)
- {
- motorL.Set_phase(L);
- motorL.Set_target(0);
- motorR.Set_phase(R);
- motorR.Set_target(slow_rpm);
- printf("turnL\n");
- }
- else
- {
- motorL.Set_phase(L);
- motorL.Set_target(slow_rpm);
- motorR.Set_phase(R);
- motorR.Set_target(0);
- printf("turnR\n");
- }
- }
- else
- {
- if (reflectorFM > 0.4)
- {
- motorL.Set_phase(L);
- motorL.Set_target(fast_rpm);
- motorR.Set_phase(R);
- motorR.Set_target(fast_rpm);
- }
- else
- {
- if (reflectorBL.read() >= reflectorBR.read())
- {
- float voltage_gap = reflectorBL.read() - reflectorBR.read();
- motorR.Set_phase(R);
- motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
- motorL.Set_phase(L);
- motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
- gap_pre = voltage_gap;
- flag = L;
- }
- else
- {
- float voltage_gap = reflectorBR.read() - reflectorBL.read();
- motorR.Set_phase(R);
- motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
- motorL.Set_phase(L);
- motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
- gap_pre = voltage_gap;
- flag = R;
- }
- }
- }
+
+ float l = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) );
+ float dv = K*l;
+ float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size );
+
+ motorL.Set_target( standard_rpm + drpm );
+ motorR.Set_target( standard_rpm - drpm );
+
printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
wait(INTVAL_REFLECTOR);
- }*/
+ }
}
void odometry()
{
- const float pi = 3.14159265359;
- const float gear_ratio = 1 / 38.2;
- const float tire_size = 0.57 / 2;
-
static float v_pre = 0.0;
static float x_dot_pre = 0.0;
static float y_dot_pre = 0.0;
@@ -203,7 +174,7 @@
x_dot_pre = x;
y_dot_pre = y;
- fast_rpm = 1500;
+ fast_rpm = 1200;
standard_rpm = 1200;
slow_rpm = 1200;
@@ -274,8 +245,6 @@
pg2.rise(&count_pg2);
pg2.fall(&count_pg2);
-
-
while (true)
{
if (button == 0)
@@ -300,12 +269,11 @@
bmx.set_initial_mag();
bmx.set_initial_acc();
- //sensor_thread.start(update);
-
- //thread_trace.start(callback(line_trace));
+ sensor_thread.start(callback(update));
+ thread_trace.start(callback(line_trace));
thread_motor.start(callback(control_motor));
- /*while (true)
+ while (true)
{
if (button == 0)
{
@@ -319,5 +287,5 @@
bmx.set_initial_acc();
lock = false;
}
- }*/
+ }
}
\ No newline at end of file