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:
- 9:87df9f0ec367
- Parent:
- 8:1c1c72c69af4
--- a/main.cpp Wed Jan 15 02:31:36 2020 +0000
+++ b/main.cpp Thu Jan 16 05:09:25 2020 +0000
@@ -1,11 +1,6 @@
-//本命
#include "mbed.h"
#include "motor.h"
-#include "BMX055.h"
-#include "way.h"
-BMX055 bmx(D0, D1);
-DigitalIn button(D2);
DigitalOut motor_mode(D9);
DigitalOut LED(D10);
Motor motorL(D5, D6);
@@ -20,14 +15,9 @@
Thread thread_trace;
Thread thread_motor;
-Thread sensor_thread;
-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;
+const float INTVAL_REFLECTOR(0.01);//0.01);
+const float INTVAL_MOTOR(0.01);
float dist = 0.0;
float x = 0.0;
@@ -36,7 +26,7 @@
float standard_rpm = 0;
float slow_rpm = 0;
-bool lock = false;
+int curve = 0;
void count_pg1()
{
@@ -54,9 +44,8 @@
//const float standard_rpm = 0;
//const float slow_rpm = 0;
static float gap_pre = 0.0;
- const float KP = 850;
- const float KD = 3000;
- const float K = 0.01;
+ const float KP = 700;
+ const float KD = 100;
const int L = 0;
const int R = 1;
@@ -65,144 +54,160 @@
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 (1)
{
- /*
- if (reflectorFL>0.15)
- {
- flag=L;
- }
- if (reflectorFR>0.15)
+ if(true)//curve==0)
{
- flag=R;
- }
-
- if ( reflectorFM<=0.4 && reflectorFL<=0.15 && reflectorFR<=0.15 )
- {
- if (flag==L)
+ if(reflectorFL > 0.1)
+ {
+ flag = L;
+ }
+ if(reflectorFR > 0.1)
{
- motorL.Set_phase(L);
- motorL.Set_target(0);
- motorR.Set_phase(R);
- motorR.Set_target(slow_rpm);
- printf("turnL\n");
+ flag = R;
+ }
+
+ if( reflectorFM<=0.4 )
+ {
+ if( flag == L )
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(0);
+ motorR.Set_phase(R);
+ motorR.Set_target(standard_rpm);
+ }
+ else
+ {
+ motorL.Set_phase(L);
+ motorL.Set_target(standard_rpm);
+ motorR.Set_phase(R);
+ motorR.Set_target(0);
+ }
}
else
{
- motorL.Set_phase(L);
- motorL.Set_target(slow_rpm);
- motorR.Set_phase(R);
- motorR.Set_target(0);
- printf("turnR\n");
+ if( reflectorFL>0.1 && reflectorFR>0.1 )
+ {
+ 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;
+ }
+ 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;
+ }
+ }
}
}
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 )
+ if( reflectorFM<=0.4 )
+ {
+ if( flag == L )
{
- 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_phase(L);
+ motorL.Set_target(0);
+ motorR.Set_phase(R);
+ motorR.Set_target(standard_rpm);
}
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_phase(L);
+ motorL.Set_target(standard_rpm);
+ motorR.Set_phase(R);
+ motorR.Set_target(0);
+ }
}
- }*/
-
- float gap_min = 0.0;
- int i_min = 0;
- for (int i = 0; i < 478; i++)
- {
- float dist_gap = dist - way[i][2];
- if (dist_gap < gap_min)
+ else
{
- gap_min = dist_gap;
- i_min = i;
+ if( reflectorFL>0.1 && reflectorFR>0.1 )
+ {
+ 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;
+ }
+ 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;
+ }
+ }
}
}
- 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);
+ //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 = 57.0 / 2;
+
static float v_pre = 0.0;
- static float x_dot_pre = 0.0;
- static float y_dot_pre = 0.0;
static float dist_count = 0.0;
float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
float v = (vl + vr) / 2;
- float x_dot = v * cos(bmx.get_azimuth_machineframe());
- float y_dot = v * sin(bmx.get_azimuth_machineframe());
dist += (v + v_pre) * INTVAL_MOTOR / 2;
dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
- x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
- y += (y_dot + y_dot_pre) * INTVAL_MOTOR / 2;
v_pre = v;
- x_dot_pre = x;
- y_dot_pre = y;
- fast_rpm = 1200;
- standard_rpm = 1200;
- slow_rpm = 1200;
-
- /*
- if (dist<=2000)
+ if (dist<=6850)
{
- fast_rpm = 2500;
- standard_rpm = 2000;
- slow_rpm = 1200;
+ fast_rpm = 5000;
+ standard_rpm = 4500;
+ curve = 0;
}
- else if(dist<=2400)
+ else if(dist<=7350)
{
- fast_rpm = 2000;
- standard_rpm = 1500;
- slow_rpm = 1200;
- }
- else if(dist<=4400)
- {
- fast_rpm = 2500;
- standard_rpm = 2000;
- slow_rpm = 1200;
+ fast_rpm = 4000;
+ standard_rpm = 4000;
+ curve = 0;
}
else
{
- fast_rpm = 1500;
- standard_rpm = 1200;
- slow_rpm = 1200;
- }*/
+ fast_rpm = 5000;
+ standard_rpm = 4500;
+ curve = 0;
+ }
//printf("%f\n", dist);
@@ -224,68 +229,16 @@
}
}
-void update()
-{
- while (true)
- {
- if (!lock)
- {
- bmx.Update_posture();
- }
- }
-}
-
int main()
-{
+{
motor_mode = 1;
LED = 1;
-
+
pg1.rise(&count_pg1);
pg1.fall(&count_pg1);
pg2.rise(&count_pg2);
pg2.fall(&count_pg2);
-
- while (true)
- {
- if (button == 0)
- {
- wait(1);
- break;
- }
- }
-
- bmx.mag_calibration(button);
-
- printf("準備完了\n");
- while (true)
- {
- if (button == 0)
- {
- wait(1);
- break;
- }
- }
-
- bmx.set_initial_mag();
- bmx.set_initial_acc();
-
- sensor_thread.start(callback(update));
+
thread_trace.start(callback(line_trace));
thread_motor.start(callback(control_motor));
-
- while (true)
- {
- if (button == 0)
- {
- bmx.posture.w = 1;
- bmx.posture.x = 0;
- bmx.posture.y = 0;
- bmx.posture.z = 0;
- lock = true;
- wait(0.1);
- bmx.set_initial_mag();
- bmx.set_initial_acc();
- lock = false;
- }
- }
}
\ No newline at end of file