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.
main.cpp
- Committer:
- kamorei
- Date:
- 2018-10-26
- Revision:
- 0:8b8f5dba70e9
- Child:
- 1:905436937f78
File content as of revision 0:8b8f5dba70e9:
//直線だけP制御したら直線でブレなくなるかもしれないので作りました #include "mbed.h" DigitalOut ledL( PTB8); DigitalOut ledR( PTE5); BusOut ledLL( PTB8, PTB9); BusOut ledRR( PTE5, PTE4); //わざと右から書いてます AnalogIn sensorR( PTB1); AnalogIn sensorL( PTB3); AnalogIn sensorCR( PTB0); AnalogIn sensorCL( PTB2); //モータ1 BusOut Mlefti(PTA1, PTA2); PwmOut Mleftp(PTD4); //モータ2 BusOut Mrighti(PTC0, PTC7); PwmOut Mrightp(PTA12); float white = 0.6, black = 0.08, gray = 0.3; //値をぶち込む float whiteR = 0.02, blackR = 0.008, grayR = 0.015; //弱いセンサ用 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR float kp = 0.5; //Pゲイン float pr, pl; int i = 0, j = 0; void turn_right(){ Mlefti = 2; Mleftp = 0.7f; Mrighti = 1; Mrightp = 0.7f; ledR = 1; ledL = 0; } void turn_left(){ Mrighti = 2; Mrightp = 0.7f; Mlefti = 1; Mleftp = 0.7f; ledR = 0; ledL = 1; } void go_straight( float r, float l){ Mrighti = 2; Mrightp = (kp * pr + 0.5) * 1.0f; Mlefti = 2; Mleftp = (kp * pl + 0.5) * 1.0f; ledR = 0; ledL = 0; } void go_back( float r, float l){ Mrighti = 1; Mrightp = (kp * pr + 0.5) * 1.0f; Mlefti = 1; Mleftp = (kp * pl + 0.5) * 1.0f; ledR = 0; ledL = 0; } void stop_point(){ Mrighti = 0; Mlefti = 0; ledRR = 2; ledLL = 2; } int main() { go_straight(); wait(0.2); while( i < 3){ while(1) { sensor[0] = sensorL.read(); sensor[1] = sensorCL.read(); sensor[2] = sensorCR.read(); sensor[3] = sensorR.read(); pr = (sensor[2] - black) / (white - black); pl = (sensor[1] - black) / (white - black); if( sensor[2] <= gray) turn_right(); else if( sensor[1] <= gray) turn_left(); else if( sensor[1] <= gray && sensor[2] <= gray){ if(ledR == 1) turn_right; else turn_left; } else go_straight( pr, pl); if( sensor[0] <= black && sensor[3] <= blackR){ stop_point(); break; } } i++; if( i < 3){ /* while(1){ sensor[0] = sensorL.read(); sensor[1] = sensorCL.read(); sensor[2] = sensorCR.read(); sensor[3] = sensorR.read(); pr = (sensor[2] - black) / (white - black); pl = (sensor[1] - black) / (white - black); if( sensor[0] <= black && sensor[3] <= blackR){ stop_point(); break; } else go_back( pr, pl); } */ for( j = 0; j < 2;){ turn_left(); if( sensor[2] <= black) j++; } } } }