p制御(失敗)

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include <stdint.h>
00004 #include "platform/mbed_thread.h"
00005 
00006 RawSerial pc(USBTX, USBRX);
00007 
00008 AnalogIn   ss1(p20);        // ライントレースセンサ(左)
00009 AnalogIn   ss2(p19);        // ライントレースセンサ
00010 AnalogIn   ss3(p18);       // ライントレースセンサ
00011 AnalogIn   ss4(p17);       // ライントレースセンサ
00012 AnalogIn   ss5(p16);       // ライントレースセンサ(右)
00013 PwmOut      motorR2(p21);   // 右モーター後退
00014 PwmOut      motorR1(p22);   // 右モーター前進
00015 PwmOut      motorL2(p23);   // 左モーター後退
00016 PwmOut      motorL1(p24);   // 左モーター前進
00017 
00018 
00019 void trace();
00020 
00021 /* ライントレーススレッド */
00022 void trace(){
00023     while(1){ 
00024             /* 各センサー値読み取り */
00025             float sensor1 = ss1;
00026             float sensor2 = ss2;
00027             float sensor3 = ss3;
00028             float sensor4 = ss4;
00029             float sensor5 = ss5;
00030             float speed;
00031             pc.printf("%f %f %f %f %f\r\n",sensor1, sensor2, sensor3, sensor4, sensor5); 
00032             speed=(0.3-sensor3)*0.33;
00033             pc.printf("%f\r\n",speed);
00034             if((sensor2-sensor4)>0.01||(sensor1-sensor4)>0.01){
00035                 motorR1 = 0;  
00036                 motorR2 = 0.3 + speed;                
00037                 motorL1 = 0.3 - speed;  
00038                 motorL2 = 0; 
00039             }else if((sensor4-sensor2)>=0.01||(sensor5-sensor2)>=0.01){
00040                 motorR1 = 0;  
00041                 motorR2 = 0.3 - speed;                
00042                 motorL1 = 0.3 + speed;  
00043                 motorL2 = 0; 
00044             }else{
00045                 
00046             }
00047             /*if(sensor2>=sensor4){
00048                 motorR1 = 0;  
00049                 motorR2 = 0.6 + 制御量;                
00050                 motorL1 = 0.6 - 制御量;  
00051                 motorL2 = 0; 
00052             }
00053             else if(sensor2<sensor4){
00054                 motorR1 = 0;  
00055                 motorR2 = 0.6 - 制御量;                
00056                 motorL1 = 0.6 + 制御量;  
00057                 motorL2 = 0; 
00058             }*/
00059     }
00060             
00061 }
00062 int main(){
00063     trace();
00064 }