p制御(失敗)

main.cpp

Committer:
yangtzuli
Date:
2020-08-31
Revision:
0:2c87f9a531df

File content as of revision 0:2c87f9a531df:

#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();
}