Diff: main.cpp

Revision:

0:2c87f9a531df

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 31 07:54:01 2020 +0000
@@ -0,0 +1,64 @@
+#include "mbed.h"
+#include "rtos.h"
+#include <stdint.h>
+#include "platform/mbed_thread.h"
+
+RawSerial pc(USBTX, USBRX);
+
+AnalogIn   ss1(p20);        // ライントレースセンサ(左)
+AnalogIn   ss2(p19);        // ライントレースセンサ
+AnalogIn   ss3(p18);       // ライントレースセンサ
+AnalogIn   ss4(p17);       // ライントレースセンサ
+AnalogIn   ss5(p16);       // ライントレースセンサ(右)
+PwmOut      motorR2(p21);   // 右モーター後退
+PwmOut      motorR1(p22);   // 右モーター前進
+PwmOut      motorL2(p23);   // 左モーター後退
+PwmOut      motorL1(p24);   // 左モーター前進
+
+
+void trace();
+
+/* ライントレーススレッド */
+void trace(){
+    while(1){ 
+            /* 各センサー値読み取り */
+            float sensor1 = ss1;
+            float sensor2 = ss2;
+            float sensor3 = ss3;
+            float sensor4 = ss4;
+            float sensor5 = ss5;
+            float speed;
+            pc.printf("%f %f %f %f %f\r\n",sensor1, sensor2, sensor3, sensor4, sensor5); 
+            speed=(0.3-sensor3)*0.33;
+            pc.printf("%f\r\n",speed);
+            if((sensor2-sensor4)>0.01||(sensor1-sensor4)>0.01){
+                motorR1 = 0;  
+                motorR2 = 0.3 + speed;                
+                motorL1 = 0.3 - speed;  
+                motorL2 = 0; 
+            }else if((sensor4-sensor2)>=0.01||(sensor5-sensor2)>=0.01){
+                motorR1 = 0;  
+                motorR2 = 0.3 - speed;                
+                motorL1 = 0.3 + speed;  
+                motorL2 = 0; 
+            }else{
+                
+            }
+            /*if(sensor2>=sensor4){
+                motorR1 = 0;  
+                motorR2 = 0.6 + 制御量;                
+                motorL1 = 0.6 - 制御量;  
+                motorL2 = 0; 
+            }
+            else if(sensor2<sensor4){
+                motorR1 = 0;  
+                motorR2 = 0.6 - 制御量;                
+                motorL1 = 0.6 + 制御量;  
+                motorL2 = 0; 
+            }*/
+    }
+            
+}
+int main(){
+    trace();
+}
\ No newline at end of file