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:
- 5:17314fc7b175
- Parent:
- 4:5588f67b8c48
- Child:
- 6:c024519cce11
--- a/main.cpp Mon Jan 13 03:54:12 2020 +0000
+++ b/main.cpp Mon Jan 13 07:07:47 2020 +0000
@@ -1,7 +1,11 @@
//本命
#include "mbed.h"
#include "motor.h"
+#include "BMX055.h"
+BMX055 bmx(D0, D1);
+
+DigitalIn button(D2);
DigitalOut motor_mode(D9);
DigitalOut LED(D10);
Motor motorL(D3, D4);
@@ -17,20 +21,25 @@
//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_MOTOR (0.25);
+const float INTVAL_REFLECTOR(0.01);
+const float INTVAL_MOTOR(0.25);
float dist = 0.0;
float x = 0.0;
float y = 0.0;
-float fast_rpm = 0;
+float fast_rpm = 0;
float standard_rpm = 0;
-float slow_rpm = 0;
+float slow_rpm = 0;
+
+bool lock = false;
void count_pg1()
{
@@ -40,7 +49,7 @@
void count_pg2()
{
motorR.count();
-}
+}
void line_trace()
{
@@ -52,124 +61,152 @@
const float KD = 3000;
const int L = 0;
const int R = 1;
-
+
int flag;
-
+
motorL.Set_phase(L);
motorR.Set_phase(R);
-
- while(1)
+
+
+
+ float target_azimuth = 0.0;
+ float current_azimuth = 0.0;
+ float curvature = 0.0;
+
+ float tread = 0.097;
+
+
+
+ while (true)
+ {
+ current_azimuth = bmx.get_azimuth_machineframe();
+
+ if(current_azimuth>3.1416)
+ {
+ current_azimuth-=6.2832;
+ }
+
+ curvature = 0.01 / (target_azimuth - current_azimuth);
+
+ //motorL.Set_target(3000 - (1 - tread/curvature));
+ //motorR.Set_target(3000 - (1 + tread/curvature));
+
+ 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<n; i++)
+ for (int i = 0; i < 500; i++)///////////////////n; i++)
{
float dist_gap = dist - way[i][3];
- if(dist_gap < gap_min)
+ if (dist_gap < gap_min)
{
gap_min = dist_gap;
i_min = i;
}
}
-
-
- /*
- if(reflectorFL > 0.1)
+
+
+
+ if (reflectorFL > 0.1)
{
flag = L;
}
- if(reflectorFR > 0.1)
+ if (reflectorFR > 0.1)
{
flag = R;
}
- //if( reflectorFM<=0.4 && reflectorFL<=0.1 && reflectorFR<=0.1 )
- if(true)
- {
- //if( flag == L )
- if(true)
+ if (reflectorFM <= 0.4 && reflectorFL <= 0.1 && reflectorFR <= 0.1)
+ if (true)
{
- motorL.Set_phase(L);
- motorL.Set_target(0);
- motorR.Set_phase(R);
- motorR.Set_target(slow_rpm);
- //printf("turnL\n");
+ 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
{
- 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() )
+ if (reflectorFM > 0.4)
{
- float voltage_gap = reflectorBL.read() - reflectorBR.read();
+ motorL.Set_phase(L);
+ motorL.Set_target(fast_rpm);
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;
+ motorR.Set_target(fast_rpm);
}
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;
+ 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;
+ }
}
}
- }*/
- //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;
+ 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;
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();
- float y_dot = v*sin();
-
- 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 += ( x_dot + x_dot_pre ) * INTVAL_MOTOR/2;
+ 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 += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
v_pre = v;
x_dot_pre = x;
y_dot_pre = y;
-
- fast_rpm = 1500;
+
+ fast_rpm = 1500;
standard_rpm = 1200;
- slow_rpm = 1200;
-
+ slow_rpm = 1200;
+
/*
if (dist<=2000)
{
@@ -195,10 +232,10 @@
standard_rpm = 1200;
slow_rpm = 1200;
}*/
-
+
//printf("%f\n", dist);
-
- if( dist_count >= 200 )
+
+ if (dist_count >= 200)
{
LED = !LED;
dist_count -= 200;
@@ -207,17 +244,28 @@
void control_motor()
{
- while(1)
+ while (1)
{
motorL.drive();
motorR.drive();
odometry();
wait(INTVAL_MOTOR);
}
-}
+}
+
+void update()
+{
+ while (true)
+ {
+ if (!lock)
+ {
+ bmx.Update_posture();
+ }
+ }
+}
int main()
-{
+{
motor_mode = 1;
LED = 1;
@@ -225,7 +273,51 @@
pg1.fall(&count_pg1);
pg2.rise(&count_pg2);
pg2.fall(&count_pg2);
-
- thread_trace.start(callback(line_trace));
+
+
+
+ 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(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