mbed_robotcar / Mbed OS sensor_test3

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@0:d41a03c442f7, 2020-07-09 (annotated)

Committer:
naoki_fukushima
Date:
Thu Jul 09 03:08:43 2020 +0000
Revision:
0:d41a03c442f7
motor control program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
naoki_fukushima 0:d41a03c442f7 1 #include "mbed.h"
naoki_fukushima 0:d41a03c442f7 2 #include "rtos.h"
naoki_fukushima 0:d41a03c442f7 3
naoki_fukushima 0:d41a03c442f7 4 DigitalIn sensor1(p20); //センサ1
naoki_fukushima 0:d41a03c442f7 5 DigitalIn sensor2(p19); //センサ2
naoki_fukushima 0:d41a03c442f7 6
naoki_fukushima 0:d41a03c442f7 7 DigitalOut ENA(p21); //ENA（右モーター）
naoki_fukushima 0:d41a03c442f7 8 DigitalOut IN1(p22); //IN1（右モーター）
naoki_fukushima 0:d41a03c442f7 9 DigitalOut IN2(p23); //IN2（右モーター）
naoki_fukushima 0:d41a03c442f7 10 DigitalOut IN3(p24); //IN3（左モーター）
naoki_fukushima 0:d41a03c442f7 11 DigitalOut IN4(p25); //IN4（左モーター）
naoki_fukushima 0:d41a03c442f7 12 DigitalOut ENB(p26); //ENB（左モーター）
naoki_fukushima 0:d41a03c442f7 13
naoki_fukushima 0:d41a03c442f7 14 int value1; //センサ1の値をintとして所持
naoki_fukushima 0:d41a03c442f7 15 int value2; //センサ2の値をintとして所持
naoki_fukushima 0:d41a03c442f7 16
naoki_fukushima 0:d41a03c442f7 17 //直進
naoki_fukushima 0:d41a03c442f7 18 void advance(){
naoki_fukushima 0:d41a03c442f7 19 ENA=1;
naoki_fukushima 0:d41a03c442f7 20 IN1=1;
naoki_fukushima 0:d41a03c442f7 21 IN2=0;
naoki_fukushima 0:d41a03c442f7 22 ENB=1;
naoki_fukushima 0:d41a03c442f7 23 IN3=1;
naoki_fukushima 0:d41a03c442f7 24 IN4=0;
naoki_fukushima 0:d41a03c442f7 25 printf("両センサ反応(sensor1=%d,sensor2=%d)\n\r",value1,value2);
naoki_fukushima 0:d41a03c442f7 26 }
naoki_fukushima 0:d41a03c442f7 27
naoki_fukushima 0:d41a03c442f7 28 //右折
naoki_fukushima 0:d41a03c442f7 29 void right(){
naoki_fukushima 0:d41a03c442f7 30 ENA=1;
naoki_fukushima 0:d41a03c442f7 31 IN1=1;
naoki_fukushima 0:d41a03c442f7 32 IN2=0;
naoki_fukushima 0:d41a03c442f7 33 ENB=1;
naoki_fukushima 0:d41a03c442f7 34 IN3=0;
naoki_fukushima 0:d41a03c442f7 35 IN4=1;
naoki_fukushima 0:d41a03c442f7 36 printf("右センサ反応(sensor1=%d,sensor2=%d)\n\r",value1,value2);
naoki_fukushima 0:d41a03c442f7 37 }
naoki_fukushima 0:d41a03c442f7 38
naoki_fukushima 0:d41a03c442f7 39 //左折
naoki_fukushima 0:d41a03c442f7 40 void left(){
naoki_fukushima 0:d41a03c442f7 41 ENA=1;
naoki_fukushima 0:d41a03c442f7 42 IN1=0;
naoki_fukushima 0:d41a03c442f7 43 IN2=1;
naoki_fukushima 0:d41a03c442f7 44 ENB=1;
naoki_fukushima 0:d41a03c442f7 45 IN3=1;
naoki_fukushima 0:d41a03c442f7 46 IN4=0;
naoki_fukushima 0:d41a03c442f7 47 printf("左センサ反応(sensor1=%d,sensor2=%d)\n\r",value1,value2);
naoki_fukushima 0:d41a03c442f7 48 }
naoki_fukushima 0:d41a03c442f7 49
naoki_fukushima 0:d41a03c442f7 50 //停止
naoki_fukushima 0:d41a03c442f7 51 void stop(){…
naoki_fukushima 0:d41a03c442f7 52 ENA=0;
naoki_fukushima 0:d41a03c442f7 53 IN1=0;
naoki_fukushima 0:d41a03c442f7 54 IN2=0;
naoki_fukushima 0:d41a03c442f7 55 ENB=0;
naoki_fukushima 0:d41a03c442f7 56 IN3=0;
naoki_fukushima 0:d41a03c442f7 57 IN4=0;
naoki_fukushima 0:d41a03c442f7 58 printf("反応なし(sensor1=%d,sensor2=%d)\n\r",value1,value2);
naoki_fukushima 0:d41a03c442f7 59 }
naoki_fukushima 0:d41a03c442f7 60
naoki_fukushima 0:d41a03c442f7 61 //走行方法を決定
naoki_fukushima 0:d41a03c442f7 62 void run(){
naoki_fukushima 0:d41a03c442f7 63 while(1){
naoki_fukushima 0:d41a03c442f7 64 if(value1 == 0 && value2 ==0){//両センサが反応
naoki_fukushima 0:d41a03c442f7 65 advance();
naoki_fukushima 0:d41a03c442f7 66 }else if(value1 == 0){//右センサが反応
naoki_fukushima 0:d41a03c442f7 67 right();
naoki_fukushima 0:d41a03c442f7 68 }else if(value2 == 0){//左センサが反応
naoki_fukushima 0:d41a03c442f7 69 left();
naoki_fukushima 0:d41a03c442f7 70 }else if(value1 == 1 && value2 ==1){//センサ反応なし
naoki_fukushima 0:d41a03c442f7 71 stop();
naoki_fukushima 0:d41a03c442f7 72 }
naoki_fukushima 0:d41a03c442f7 73 ThisThread::sleep_for(1);
naoki_fukushima 0:d41a03c442f7 74 }
naoki_fukushima 0:d41a03c442f7 75 }
naoki_fukushima 0:d41a03c442f7 76
naoki_fukushima 0:d41a03c442f7 77 //読み込んだセンサの値をvalueに格納
naoki_fukushima 0:d41a03c442f7 78 void sensor(void const *argument){
naoki_fukushima 0:d41a03c442f7 79 value1 = sensor1;
naoki_fukushima 0:d41a03c442f7 80 value2 = sensor2;
naoki_fukushima 0:d41a03c442f7 81 }
naoki_fukushima 0:d41a03c442f7 82
naoki_fukushima 0:d41a03c442f7 83 //main関数
naoki_fukushima 0:d41a03c442f7 84 int main(){
naoki_fukushima 0:d41a03c442f7 85 Thread thread; //スレッド作成
naoki_fukushima 0:d41a03c442f7 86 RtosTimer sensor_timer(sensor,osTimerPeriodic,(void*)0); //sensor関数をRtosTimerに設定
naoki_fukushima 0:d41a03c442f7 87 sensor_timer.start(5); //sensorを5msごとに起動
naoki_fukushima 0:d41a03c442f7 88 thread.start(run); //スレッドとしてrun関数を開始
naoki_fukushima 0:d41a03c442f7 89
naoki_fukushima 0:d41a03c442f7 90 while(1){
naoki_fukushima 0:d41a03c442f7 91 ThisThread::sleep_for(5);
naoki_fukushima 0:d41a03c442f7 92 }
naoki_fukushima 0:d41a03c442f7 93
naoki_fukushima 0:d41a03c442f7 94 }