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:
- 4:5588f67b8c48
- Parent:
- 3:efc081576a33
- Child:
- 5:17314fc7b175
--- a/main.cpp Thu Dec 12 07:27:36 2019 +0000
+++ b/main.cpp Mon Jan 13 03:54:12 2020 +0000
@@ -1,12 +1,13 @@
+//本命
#include "mbed.h"
#include "motor.h"
DigitalOut motor_mode(D9);
DigitalOut LED(D10);
-Motor motorL(D5, D6);
-Motor motorR(D3, D4);
-InterruptIn pg1(D12);
-InterruptIn pg2(D11);
+Motor motorL(D3, D4);
+Motor motorR(D5, D6);
+InterruptIn pg1(D11);
+InterruptIn pg2(D12);
AnalogIn reflectorFL(A6);
AnalogIn reflectorFM(A3);
AnalogIn reflectorFR(A2);
@@ -25,6 +26,8 @@
const float INTVAL_MOTOR (0.25);
float dist = 0.0;
+float x = 0.0;
+float y = 0.0;
float fast_rpm = 0;
float standard_rpm = 0;
float slow_rpm = 0;
@@ -44,34 +47,58 @@
//const float fast_rpm = 0;
//const float standard_rpm = 0;
//const float slow_rpm = 0;
- static float pre = 0.0;
- const float KP = 700;//850;
- const float KD = 500;
+ static float gap_pre = 0.0;
+ const float KP = 5000;//850;
+ const float KD = 3000;
const int L = 0;
const int R = 1;
int flag;
- motorL.Set_phase(0);
- motorR.Set_phase(1);
+ motorL.Set_phase(L);
+ motorR.Set_phase(R);
while(1)
{
- if( reflectorFM<=0.4 && reflectorFL<=0.4 && reflectorFR<=0.4 )
- {
- if( flag == L )
+ float gap_min = 0.0;
+ int i_min = 0;
+ for(int i=0; i<n; i++)
+ {
+ float dist_gap = dist - way[i][3];
+ if(dist_gap < gap_min)
{
- motorL.Set_phase(0);
+ 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(1);
- motorR.Set_target(standard_rpm);
+ motorR.Set_phase(R);
+ motorR.Set_target(slow_rpm);
//printf("turnL\n");
}
else
{
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm);
- motorR.Set_phase(1);
+ motorL.Set_phase(L);
+ motorL.Set_target(slow_rpm);
+ motorR.Set_phase(R);
motorR.Set_target(0);
//printf("turnR\n");
}
@@ -80,116 +107,40 @@
{
if( reflectorFM > 0.4 )
{
- motorL.Set_phase(0);
+ motorL.Set_phase(L);
motorL.Set_target(fast_rpm);
- motorR.Set_phase(1);
+ motorR.Set_phase(R);
motorR.Set_target(fast_rpm);
}
else
{
- if( reflectorFL.read() >= reflectorFR.read() )
+ if( reflectorBL.read() >= reflectorBR.read() )
{
- motorR.Set_phase(1);
- motorR.Set_target(standard_rpm + KP * reflectorFL.read() + KD * (reflectorFL.read() - pre));
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm - KP * reflectorFL.read() + KD * (reflectorFL.read() - pre));
- pre = reflectorFL.read();
- flag = L;
+ 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
{
- motorR.Set_phase(1);
- motorR.Set_target(standard_rpm - KP * reflectorFR.read() + KD * (reflectorFR.read() - pre));
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm + KP * reflectorFR.read() + KD * (reflectorFR.read() - pre));
- pre = reflectorFR.read();
- flag = R;
+ 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;
}
}
- wait(INTVAL_REFLECTOR);
- }
-
- 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 line_trace()
-{
- //const float fast_rpm = 0;
- //const float standard_rpm = 0;
- //const float slow_rpm = 0;
- static float gap_pre = 0.0;
- const float KP = 700;//850;
- const float KD = 500;
- const int L = 0;
- const int R = 1;
-
- int flag;
-
- motorL.Set_phase(0);
- motorR.Set_phase(1);
-
- while(1)
- {
- if( reflectorF<=0.4 && reflectorR<=0.4 && reflectorL<= 0.4 )
- {
- if( flag == L )
- {
- motorL.Set_phase(0);
- motorL.Set_target(0);
- motorR.Set_phase(1);
- motorR.Set_target(standard_rpm);
- //printf("turnL\n");
- }
- else
- {
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm);
- motorR.Set_phase(1);
- motorR.Set_target(0);
- //printf("turnR\n");
- }
- }
- else
- {
- if( reflectorF > 0.4 )
- {
- motorL.Set_phase(0);
- motorL.Set_target(fast_rpm);
- motorR.Set_phase(1);
- motorR.Set_target(fast_rpm);
- }
- else
- {
- if( reflectorL.read() >= reflectorR.read() )
- {
- float voltage_gap = reflectorL.read() - reflectorR.read();
- motorR.Set_phase(1);
- motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
- gap_pre = voltage_gap;
- flag = L;
- }
- else
- {
- float voltage_gap = reflectorR.read() - reflectorL.read();
- motorR.Set_phase(1);
- motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
- motorL.Set_phase(0);
- motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
- gap_pre = voltage_gap;
- flag = R;
- }
- }
- wait(INTVAL_REFLECTOR);
- }
-
- //printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
- }
-}*/
-
void odometry()
{
const float pi = 3.14159265359;
@@ -197,16 +148,29 @@
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();
+ 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;
v_pre = v;
+ x_dot_pre = x;
+ y_dot_pre = y;
+ fast_rpm = 1500;
+ standard_rpm = 1200;
+ slow_rpm = 1200;
+
+ /*
if (dist<=2000)
{
fast_rpm = 2500;
@@ -230,7 +194,7 @@
fast_rpm = 1500;
standard_rpm = 1200;
slow_rpm = 1200;
- }
+ }*/
//printf("%f\n", dist);
@@ -239,11 +203,6 @@
LED = !LED;
dist_count -= 200;
}
-
- if(dist>2147483000)
- {
- dist = 0.0;
- }
}
void control_motor()