Revision 0:e9af471e2a2b, committed 2017-11-16

Comitter:

Hirotomo777

Date:

Thu Nov 16 06:15:58 2017 +0000

Commit message:

?7????????????????

Changed in this revision

diff -r 000000000000 -r e9af471e2a2b linetracer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/linetracer.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,76 @@
+#include "linetracer.h"
+#define K 0.000001 //ハンドルのききの良さを決める定数
+DigitalOut test1(PTB8);
+DigitalOut test2(PTB9);
+DigitalOut test3(PTB10);
+DigitalOut test4(PTB11);
+
+void linetracerInit(){
+    servoInit();
+    sensorInit();
+    motorInit();
+    start();
+}
+
+void linetracerLoop(){
+    float pulse;
+    pulse = 0.00155;
+    int reaction = 0;
+    int flag = 0;
+    while(1){
+        reaction = getReaction();
+        /*
+        if((reaction & 0b000011) == 0b000000){
+                flag++;
+                if(flag >= 100){
+                    wait(0.3);
+                    stop();
+                    while(1){
+                        test1 = 1;
+                    }
+                }
+        }
+        else{
+            flag = 0;
+            test1 = 0;
+        }
+        */
+        
+        
+        if((reaction & 0b100000) == 0b100000){
+            rotate(handle,pulse - K,&pulse);
+            test1 = 1;
+        }
+        
+        
+        if((reaction & 0b000100) == 0b000100){
+            rotate(handle,pulse + K,&pulse);
+            test2 = 1;
+        }
+        
+        if((reaction & 0b010000) == 0b010000){
+            rotate(handle,pulse - K / 3.0,&pulse);
+            test3 = 1;
+        }
+        
+        if((reaction & 0b001000) == 0b001000){
+            rotate(handle,pulse + K / 3.0,&pulse);
+            test4 = 1;
+        }
+        
+        test1 = 0;
+        test2 = 0;
+        test3 = 0;
+        test4 = 0;
+        
+        /*
+        if((reaction & 0b010100) == 0b000000){
+            rotate(handle,pulse + K * 0.5,&pulse);
+        }
+        
+        if((reaction & 0b1010000) == 0b000000){
+            rotate(handle,pulse - K * 0.5,&pulse);
+        }
+        */
+    }
+}

diff -r 000000000000 -r e9af471e2a2b linetracer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/linetracer.h	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,4 @@
+#include "motor.h"
+#include "sensor.h"
+void linetracerInit();
+void linetracerLoop();

diff -r 000000000000 -r e9af471e2a2b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+#include "linetracer.h"
+
+int main() {
+    linetracerInit();
+    linetracerLoop();
+    return 0;
+}
+
+

diff -r 000000000000 -r e9af471e2a2b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b484a57bc302
\ No newline at end of file

diff -r 000000000000 -r e9af471e2a2b motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,77 @@
+
+#include "motor.h"
+BusOut leftc(PTA1,PTA2);
+PwmOut leftp(PTD4);
+
+BusOut rightc(PTC0,PTC7);
+PwmOut rightp(PTA12);
+
+PwmOut handle(PTC9);
+
+//start code for servo
+void servoInit(){
+    handle.period(0.020);
+    handle.pulsewidth(0.00155);
+    wait(0.5);
+}
+
+
+void rotate(PwmOut servo,float ppulse,float *npulse){
+    if(ppulse > *npulse){
+        if(ppulse > 0.002){
+            ppulse = 0.002;
+        }
+        servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms
+        wait(1000.0 * 0.06 * ((ppulse - (*npulse)) / 0.1));
+    }
+    else {
+        if(ppulse < 0.0011){
+            ppulse = 0.0011;
+        }
+        servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms
+        wait(1000.0 * 0.06 * (((*npulse) - ppulse) / 0.1));
+    }
+    *npulse = ppulse;
+}
+
+/*
+float rotate(PwmOut servo,float ppulse){
+    static float npulse = 0.00155;
+    if(ppulse > npulse){
+        if(ppulse > 0.002){
+            ppulse = 0.002;
+        }
+        servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms
+        wait(1000.0 * 0.06 * ((ppulse - (npulse)) / 0.1));
+    }
+    else {
+        if(ppulse < 0.0011){
+            ppulse = 0.0011;
+        }
+        servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms
+        wait(1000.0 * 0.06 * (((npulse) - ppulse) / 0.1));
+    }
+    npulse = ppulse;
+    return npulse;
+}
+*/
+//end
+
+//start code for DCmotor
+void motorInit(){
+    leftc = 0;
+    leftp = 1.0f;
+    rightc = 0;
+    rightp = 1.0f;
+}
+
+void start(){
+    leftc = 1;
+    rightc = 1;
+}
+
+void stop(){
+    leftc = 0;
+    rightc = 0;
+}
+//end
\ No newline at end of file

diff -r 000000000000 -r e9af471e2a2b motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+extern PwmOut handle;
+void motorInit();
+void rotate(PwmOut servo,float ppulse,float *npulse);
+//float rotate(PwmOut servo,float ppulse);
+void servoInit();
+void start();
+void stop();
+void control(PwmOut left,PwmOut right);
\ No newline at end of file

diff -r 000000000000 -r e9af471e2a2b sensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensor.cpp	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,131 @@
+#include "sensor.h"
+#define threshold1 0.005
+#define threshold2 0.005
+/*
+AnalogIn sensorfl(PTB0);
+AnalogIn sensorfr(PTB1);
+AnalogIn sensorbl(PTB2);
+AnalogIn sensorbr(PTB3);
+*/
+AnalogIn sensor1(PTB0);  //左から数えて1,2,3,4
+AnalogIn sensor2(PTB1);
+AnalogIn sensor3(PTB2);
+AnalogIn sensor4(PTB3);
+
+DigitalIn sensorl(PTD6);
+DigitalIn sensorr(PTD7);
+
+Timer t;
+
+static float th1 = 0.0;
+static float th2 = 0.0;
+static float th3 = 0.0;
+static float th4 = 0.0;
+
+void sensorInit(){
+    float th1max = sensor1;
+    float th1min = sensor1;
+    float th2max = sensor2;
+    float th2min = sensor2;
+    float th3max = sensor3;
+    float th3min = sensor3;
+    float th4max = sensor4;
+    float th4min = sensor4;
+    handle.pulsewidth(0.001);
+    t.start();
+    while(1){
+        if(t.read() > 1.0){
+            break;
+        }
+        if(th1max < sensor1){
+            th1max = sensor1;
+        }
+        if(th1min > sensor1){
+            th1min = sensor1;
+        }
+        if(th2max < sensor2){
+            th2max = sensor2;
+        }
+        if(th2min > sensor2){
+            th2min = sensor2;
+        }
+        if(th3max < sensor3){
+            th3max = sensor3;
+        }
+        if(th3min > sensor3){
+            th3min = sensor3;
+        }
+        if(th4max < sensor4){
+            th4max = sensor4;
+        }
+        if(th4min > sensor4){
+            th4min = sensor4;
+        }
+    }
+    //wait(1.0);
+    t.stop();
+    t.reset();
+    handle.pulsewidth(0.0021);
+    t.start();
+    while(1){
+        if(t.read() > 1.0){
+            break;
+        }
+        if(th1max < sensor1){
+            th1max = sensor1;
+        }
+        if(th1min > sensor1){
+            th1min = sensor1;
+        }
+        if(th2max < sensor2){
+            th2max = sensor2;
+        }
+        if(th2min > sensor2){
+            th2min = sensor2;
+        }
+        if(th3max < sensor3){
+            th3max = sensor3;
+        }
+        if(th3min > sensor3){
+            th3min = sensor3;
+        }
+        if(th4max < sensor4){
+            th4max = sensor4;
+        }
+        if(th4min > sensor4){
+            th4min = sensor4;
+        }
+    }
+    th1 = (th1max + th1min) / 2.0;
+    th2 = (th2max + th2min) / 2.0;
+    th3 = (th3max + th3min) / 2.0;
+    th4 = (th4max + th4min) / 2.0;
+    t.stop();
+    //wait(1.0);
+    handle.pulsewidth(0.00155);
+    wait(0.5);
+}
+
+int getReaction(){
+    int tmp = 0;
+    if(sensor1 < th1){
+        tmp += 0b100000;
+    }
+    
+    if(sensor2 < th2){
+        //tmp += 0b010000;
+        tmp += 0b000000;
+    }
+    if(sensor3 < th3){
+        tmp += 0b000000;
+        //死んでる
+    }
+    
+    if(sensor4 < th4){
+        tmp += 0b000100;
+    }
+    
+    tmp += 0b000010 * sensorl;
+    tmp += 0b000001 * sensorr;
+    return tmp;
+}
\ No newline at end of file

diff -r 000000000000 -r e9af471e2a2b sensor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensor.h	Thu Nov 16 06:15:58 2017 +0000
@@ -0,0 +1,4 @@
+#include "mbed.h"
+#include "motor.h"
+void sensorInit();
+int getReaction();
\ No newline at end of file