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:
- 3:efc081576a33
- Parent:
- 2:09ea66e396c1
- Child:
- 4:5588f67b8c48
diff -r 09ea66e396c1 -r efc081576a33 main.cpp
--- a/main.cpp Wed Dec 04 02:32:00 2019 +0000
+++ b/main.cpp Thu Dec 12 07:27:36 2019 +0000
@@ -2,14 +2,19 @@
#include "motor.h"
DigitalOut motor_mode(D9);
-DigitalOut LED(A6);
+DigitalOut LED(D10);
Motor motorL(D5, D6);
Motor motorR(D3, D4);
InterruptIn pg1(D12);
InterruptIn pg2(D11);
-AnalogIn reflectorF(A2);
-AnalogIn reflectorL(A1);
-AnalogIn reflectorR(A0);
+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;
@@ -20,7 +25,9 @@
const float INTVAL_MOTOR (0.25);
float dist = 0.0;
+float fast_rpm = 0;
float standard_rpm = 0;
+float slow_rpm = 0;
void count_pg1()
{
@@ -30,14 +37,16 @@
void count_pg2()
{
motorR.count();
-}
+}
void line_trace()
{
- //const float fast_rpm = 2000;
- //const float standard_rpm = 1200;
- //const float slow_rpm = 1000;
- const float K = 750;//850;
+ //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;
const int L = 0;
const int R = 1;
@@ -48,22 +57,97 @@
while(1)
{
- if( /*reflectorF<=0.4 &&*/ reflectorR<=0.4 && reflectorL<= 0.4 )
+ if( reflectorFM<=0.4 && reflectorFL<=0.4 && reflectorFR<=0.4 )
{
if( flag == L )
{
- motorL.Set_phase(0);//1);
- motorL.Set_target(0);//slow_rpm);
+ motorL.Set_phase(0);
+ motorL.Set_target(0);
motorR.Set_phase(1);
- motorR.Set_target(standard_rpm);//slow_rpm);
+ motorR.Set_target(standard_rpm);
//printf("turnL\n");
}
else
{
motorL.Set_phase(0);
- motorL.Set_target(standard_rpm);//slow_rpm);
- motorR.Set_phase(1);//0);
- motorR.Set_target(0);//slow_rpm);
+ motorL.Set_target(standard_rpm);
+ motorR.Set_phase(1);
+ motorR.Set_target(0);
+ //printf("turnR\n");
+ }
+ }
+ else
+ {
+ if( reflectorFM > 0.4 )
+ {
+ motorL.Set_phase(0);
+ motorL.Set_target(fast_rpm);
+ motorR.Set_phase(1);
+ motorR.Set_target(fast_rpm);
+ }
+ else
+ {
+ if( reflectorFL.read() >= reflectorFR.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;
+ }
+ 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;
+ }
+ }
+ wait(INTVAL_REFLECTOR);
+ }
+
+ printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
+ }
+}
+
+/*
+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");
}
}
@@ -82,18 +166,20 @@
{
float voltage_gap = reflectorL.read() - reflectorR.read();
motorR.Set_phase(1);
- motorR.Set_target(standard_rpm + K * voltage_gap);
+ motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
motorL.Set_phase(0);
- motorL.Set_target(standard_rpm - K * voltage_gap);
+ 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 - K * voltage_gap);
+ motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
motorL.Set_phase(0);
- motorL.Set_target(standard_rpm + K * voltage_gap);
+ motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
+ gap_pre = voltage_gap;
flag = R;
}
}
@@ -102,7 +188,7 @@
//printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
}
-}
+}*/
void odometry()
{
@@ -121,12 +207,32 @@
dist_count += ( v + v_pre ) * INTVAL_MOTOR/2;
v_pre = v;
- if (dist<=2000) standard_rpm = 2000;
- else if(dist<=2400) standard_rpm = 1200;
- else if(dist<=4400) standard_rpm = 2000;
- else standard_rpm = 1000;
+ if (dist<=2000)
+ {
+ fast_rpm = 2500;
+ standard_rpm = 2000;
+ slow_rpm = 1200;
+ }
+ else if(dist<=2400)
+ {
+ fast_rpm = 2000;
+ standard_rpm = 1500;
+ slow_rpm = 1200;
+ }
+ else if(dist<=4400)
+ {
+ fast_rpm = 2500;
+ standard_rpm = 2000;
+ slow_rpm = 1200;
+ }
+ else
+ {
+ fast_rpm = 1500;
+ standard_rpm = 1200;
+ slow_rpm = 1200;
+ }
- printf("%f\n", dist);
+ //printf("%f\n", dist);
if( dist_count >= 200 )
{