mech
/
Shiken_3
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:
- NSugiura
- Date:
- 2018-12-19
- Revision:
- 7:de699a50fc63
- Parent:
- 6:9bd1da494e0b
File content as of revision 7:de699a50fc63:
#include "function.h"
PwmOut pwm11(D5); //左正転
PwmOut pwm12(D1); //左逆転
PwmOut pwm21(D6); //右正転
PwmOut pwm22(D9); //右逆転
DigitalOut LED(D2);
InterruptIn button1(A2); //フォトインタラプタ左
InterruptIn button2(A1); //フォトインタラプタ右
AnalogIn in1(A6); //フォトリフレクタ左
AnalogIn in2(A4); //フォトリフレクタ真ん中
AnalogIn in3(A3); //フォトリフレクタ右
float g1 = 0.0f;//目標値（パルス数）
float g2 = 0.0f;//目標値（パルス数）
float gain1 = 0.0f;//左ゲイン
float gain2 = 0.0f;//右ゲイン
float dif1 = 0.0f;//目標との差左
float dif2 = 0.0f;//目標との差右
float thr1 = 0.10f;//フォトリフレクタのしきい値左
float thr2 = 0.40f;//フォトリフレクタのしきい値真ん中
float thr3 = 0.10f;//フォトリフレクタのしきい値右
int judge1 = 0;
int judge2 = 0;
int judge3 = 0;
int mode1 = 10;
int mode2 = 2;
int mode3 = 2;
int curve_num = 1;
int main() {
pwm11.period(0.01f); //100Hz
pwm12.period(0.01f); //100Hz
pwm21.period(0.01f); //100Hz
pwm22.period(0.01f); //100Hz
button1.rise(&flip1);
button2.rise(&flip2);
int n;//3biteで状態を表現
// flipper.attach(&flip3,0.05f);//0.05秒ごとに回転数cを更新
while(1) {
// distance = (distance1+distance2)/2;
// if(distance < 43*2*12*mode1){
/*
if( led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
LED = 1.0f;
wait(0.5f);
LED = 0;
led_count1 = 0;
led_count2 = 0;
}
*/
//////////////////floatのinをintのjudgeに変換
if(in1 > thr1){
judge1 = 1;
}
else{
judge1 = 0;
}
if(in2 > thr2){
judge2 = 1;
}
else{
judge2 = 0;
}
if(in3 > thr3){
judge3 = 1;
}
else{
judge3 = 0;
}
/////////////////
n = judge1 + judge2*2 + judge3*4;
switch(n){
case(2)://白黒白進む
g1 = 500.0f;
g2 = 500.0f;
dif1 = g1 - c1;
dif2 = g2 - c2;
pwm11.write(0.31f);
pwm12.write(0.10f);
pwm21.write(0.31f);
pwm22.write(0.10f);
break;
case(1)://黒白白左回転強
pwm11.write(0.30f);
pwm12.write(0.20f);
pwm21.write(0.40f);
pwm22.write(0.20f);
break;
case(3)://黒黒白左回転弱
pwm11.write(0.47f);
pwm12.write(0.30f);
pwm21.write(0.41f);
pwm22.write(0.30f);
break;
case(4)://白白黒右回転強
pwm11.write(0.41f);
pwm12.write(0.20f);
pwm21.write(0.30f);
pwm22.write(0.20f);
break;
case(6)://白黒黒右回転弱
pwm11.write(0.47f);
pwm12.write(0.30f);
pwm21.write(0.41f);
pwm22.write(0.30f);
break;
case(7)://黒黒黒爆走
pwm11.write(0.45f);
pwm12.write(0.10f);
pwm21.write(0.45f);
pwm22.write(0.10f);
break;
case(0)://白白白
switch(curve_num){
case(1):
pwm11.write(0.10f);
pwm12.write(0.20f);
pwm21.write(0.10f);
pwm22.write(0.20f);
while(in2 < thr2){
pwm11.write(0.30f);
pwm12.write(0.30f);
pwm21.write(0.48f);
pwm22.write(0.30f);
}
curve_num++;
pwm11.write(0.40f);
pwm12.write(0.10f);
pwm21.write(0.20f);
pwm22.write(0.45f);
break;
case(2):
pwm11.write(0.20f);
pwm12.write(0.40f);
pwm21.write(0.20f);
pwm22.write(0.40f);
while(in1 < thr1){
pwm11.write(1.00f);
pwm12.write(1.00f);
pwm21.write(0.40f);
pwm22.write(0.20f);
}
curve_num++;
while(in2 > thr2){
pwm11.write(0.80f);
pwm12.write(0.10f);
pwm21.write(0.20f);
pwm22.write(0.20f);
}
break;
case(3):
while(in1 < thr1){
pwm11.write(0.30f);
pwm12.write(0.10f);
pwm21.write(0.20f);
pwm22.write(0.20f);
}
curve_num++;
while(in1 > thr1){
pwm11.write(0.47f);
pwm12.write(0.20f);
pwm21.write(0.40f);
pwm22.write(0.20f);
}
break;
case(4):
while(in3 < thr3){
pwm11.write(0.30f);
pwm12.write(0.10f);
pwm21.write(0.20f);
pwm22.write(0.20f);
}
pwm11.write(0.10f);
pwm12.write(0.20f);
pwm21.write(0.80f);
pwm22.write(0.20f);
curve_num++;
break;
case(5):
while(in2 < thr2){
pwm11.write(0.38f);
pwm12.write(0.10f);
pwm21.write(0.20f);
pwm22.write(0.20f);
}
curve_num++;
break;
default:
pwm11.write(0.30f);
pwm12.write(0.40f);
pwm21.write(0.30f);
pwm22.write(0.40f);
break;
}
break;
default://それ以外(ないけど)
pwm11.write(0.00f);
pwm12.write(0.00f);
pwm21.write(0.00f);
pwm22.write(0.00f);
break;
}
/* else{
pwm11.write(0.00f);
pwm12.write(0.00f);
pwm21.write(0.00f);
pwm22.write(0.00f);
}*/
}
}