Compare commits
No commits in common. "6ccd5066ae90134c9c386f6bfde12782c7e76223" and "3c10dd5773605c9be750bb2e5c92740354a47dc2" have entirely different histories.
6ccd5066ae
...
3c10dd5773
3 changed files with 138 additions and 116 deletions
25
README.md
25
README.md
|
|
@ -37,9 +37,9 @@
|
|||
* ホイールベース : 約150mm
|
||||
* 車重 : 1kg以下
|
||||
* 車体材料 : タミヤ ユニバーサルプレートL
|
||||
* 使用ギアボックス :
|
||||
* ギア比 :
|
||||
* 使用モーター :
|
||||
* 使用ギアボックス : タミヤ ハイスピードギアボックスHE
|
||||
* ギア比 : 11.6:1
|
||||
* 使用モーター : 上記ギアボックスに付属するもの
|
||||
* 使用サーボモーター : Savox SC-0352(ランス用)
|
||||
|
||||
### 回路仕様 ###
|
||||
|
|
@ -47,6 +47,25 @@
|
|||
* 使用マイコン : Atmel AVR (Arduino)
|
||||
* 使用センサーアレイ : SG-2BC x 8 (ロボラボ講習会のもの)
|
||||
|
||||
### Software ###
|
||||
[count/mark]
|
||||
|
|
||||
+-><mode>
|
||||
|
|
||||
+->[lance]
|
||||
|
|
||||
| [line]
|
||||
| |
|
||||
+--+->[motor/steering]
|
||||
|
||||
mode - 全体の動作を決定
|
||||
count - ロータリーエンコーダーから得られる情報
|
||||
mark - コース上のマーカーから得られる情報
|
||||
line - コース上の線から得られる情報
|
||||
motor - 駆動用モーターの動作
|
||||
steering - ステアリングの動作
|
||||
lance - ランスの動作
|
||||
|
||||
### ソフトウェア仕様 ###
|
||||
* 使用言語 : Arduino 1.0
|
||||
|
||||
|
|
|
|||
159
hanzo4_1.ino
159
hanzo4_1.ino
|
|
@ -31,39 +31,44 @@ struct target_t{
|
|||
/**************************************/
|
||||
/* 動作パラメーター */
|
||||
|
||||
unsigned long lapDistance = 28550; //ロータリーエンコーダーのカウント数/周
|
||||
unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周
|
||||
unsigned long turnDistance = 4650; //カーブ中でのカウント数
|
||||
|
||||
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 900, { 29, 0}};
|
||||
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 900, {-32, 0}};
|
||||
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 900, { 34, 0}};
|
||||
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42, 0}};
|
||||
struct target_t targetE = {{ 500,4000}, 750, { 48, 53}};
|
||||
struct target_t targetF = {{4000,7000}, 750, { 60, 67}};
|
||||
struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 1500, { 29, 0}};
|
||||
struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 1500, {-32, 0}};
|
||||
struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 1500, { 34, 0}};
|
||||
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42, 0}};
|
||||
struct target_t targetE = {{0, lapDistance/8 }, 800, { 40, 48}};
|
||||
struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 1000, { 61, 69}};
|
||||
|
||||
int laps = 3; //最大周回数
|
||||
bool silent = false; //サイレントモード(true:ブザーを鳴らさない)
|
||||
int laps = 1; //最大周回数
|
||||
bool silent = true; //サイレントモード(true:ブザーを鳴らさない)
|
||||
|
||||
int steeringAngleMin = -30; //度
|
||||
int steeringAngleMax = 10; //度
|
||||
int steeringAngleMax = 30; //度
|
||||
|
||||
int deltaSpeedMax = 500; //最大速度差
|
||||
int speedMax = 255; //最大速度
|
||||
int speedMax = 100; //最大速度
|
||||
|
||||
int cycle = 1; //動作周波数(ms)
|
||||
float steeringKP = 3.8;
|
||||
float steeringKI = 0.0;
|
||||
float steeringKD = 0.0;
|
||||
|
||||
int errorCountThreshold = 500; //最大読み取りエラー回数(これを超えると停止する)
|
||||
float speedKP = 50.0;
|
||||
float speedKI = 0.0;
|
||||
float speedKD = 0.0;
|
||||
|
||||
int errorCountThreshold = 5000; //最大読み取りエラー回数(これを超えると停止する)
|
||||
|
||||
/**************************************/
|
||||
/* ピンの設定 */
|
||||
|
||||
// Servo : SC-1267SG
|
||||
unsigned int steeringServoCenter = 1808; //us
|
||||
unsigned int steeringServoCenter = 1810; //us
|
||||
unsigned int steeringServoMin = (steeringServoCenter-780); //us
|
||||
unsigned int steeringServoMax = (steeringServoCenter+780); //us
|
||||
|
||||
// Servo : SC-0352
|
||||
unsigned int lanceServoCenter = 1560; //us
|
||||
unsigned int lanceServoCenter = 1530; //us
|
||||
unsigned int lanceServoMin = (lanceServoCenter-780); //us
|
||||
unsigned int lanceServoMax = (lanceServoCenter+780); //us
|
||||
|
||||
|
|
@ -92,12 +97,14 @@ Servo lanceServo;
|
|||
Motor motorR;
|
||||
Motor motorL;
|
||||
|
||||
//ラインが1、地面が0、MSBが左端、LSBが右端
|
||||
//ラインが1、地面が0、LSBが左端、MSBが右端
|
||||
unsigned char sensor = 0x00;
|
||||
|
||||
int linePosition = 0;
|
||||
int lineDifferential = 0;
|
||||
long lineIntegral = 0;
|
||||
|
||||
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
||||
int deltaSpeed = 0;
|
||||
// 駆動用モーターの速度
|
||||
int speed = 0;
|
||||
|
||||
// ステアリングの角度
|
||||
|
|
@ -109,8 +116,8 @@ volatile unsigned long distance = 0;
|
|||
unsigned long modeChangedDistance = 0;
|
||||
|
||||
/**************************************/
|
||||
|
||||
/* 動作モード */
|
||||
|
||||
enum mode_t {
|
||||
MODE_STOP, //停止
|
||||
MODE_STRAIGHT, //まっすぐ進む
|
||||
|
|
@ -126,73 +133,6 @@ enum mode_t {
|
|||
|
||||
/**************************************/
|
||||
|
||||
/* センサーを読んで、車体の動作を決定する */
|
||||
void trace(){
|
||||
switch(sensor&lineSensorMask){
|
||||
default:
|
||||
errorCount++;
|
||||
return;
|
||||
case 0b01000000:
|
||||
speed = speedMax*0.3;
|
||||
deltaSpeed = -145;
|
||||
angle = -15;
|
||||
break;
|
||||
case 0b01100000:
|
||||
speed = speedMax*0.5;
|
||||
deltaSpeed = -115;
|
||||
angle = -13;
|
||||
break;
|
||||
case 0b00100000:
|
||||
speed = speedMax*0.7;
|
||||
deltaSpeed = -95;
|
||||
angle = -11;
|
||||
break;
|
||||
case 0b00110000:
|
||||
speed = speedMax*0.9;
|
||||
deltaSpeed = -75;
|
||||
angle = -9;
|
||||
break;
|
||||
case 0b00010000:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 0;
|
||||
angle = -1;
|
||||
break;
|
||||
case 0b00011000:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 0;
|
||||
angle = 0;
|
||||
break;
|
||||
case 0b00001000:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 0;
|
||||
angle = 1;
|
||||
break;
|
||||
case 0b00001100:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 0;
|
||||
angle = 2;
|
||||
break;
|
||||
case 0b00000100:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 3;
|
||||
angle = 3;
|
||||
break;
|
||||
case 0b00000110:
|
||||
speed = speedMax;
|
||||
deltaSpeed = 4;
|
||||
angle = 4;
|
||||
break;
|
||||
case 0b00000010:
|
||||
speed = 0;
|
||||
deltaSpeed = 5;
|
||||
angle = 5;
|
||||
break;
|
||||
}
|
||||
errorCount = 0;
|
||||
}
|
||||
|
||||
/**************************************/
|
||||
|
||||
void setup(){
|
||||
//サーボ
|
||||
steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
|
||||
|
|
@ -210,35 +150,35 @@ void setup(){
|
|||
pinMode(rotPin, INPUT);
|
||||
attachInterrupt(0, rot, RISING);
|
||||
//ブザー
|
||||
startBeep();
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
if(!silent) startBeep();
|
||||
/* 3秒停止 */
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
delay(3000);
|
||||
/* スタート */
|
||||
mode = MODE_STRAIGHT;
|
||||
MsTimer2::set(cycle, run);
|
||||
MsTimer2::set(1, run);
|
||||
MsTimer2::start();
|
||||
}
|
||||
|
||||
void loop(){
|
||||
scanSensor();
|
||||
sensorRead();
|
||||
}
|
||||
|
||||
/* 車体の動作を決定する */
|
||||
/* 車体の動作を決定する(1kHz) */
|
||||
void run(){
|
||||
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
|
||||
mode = MODE_STOP;
|
||||
}
|
||||
switch(mode){
|
||||
default:
|
||||
case MODE_STOP: //停止
|
||||
MsTimer2::stop();
|
||||
lanceServo.detach();
|
||||
steeringServo.detach();
|
||||
motorL.mode(STOP);
|
||||
motorR.mode(STOP);
|
||||
if(!silent) stopBeep();
|
||||
while(1){
|
||||
;
|
||||
}
|
||||
};
|
||||
break;
|
||||
case MODE_STRAIGHT: //まっすぐ進む
|
||||
trace();
|
||||
|
|
@ -303,24 +243,33 @@ void run(){
|
|||
mode = MODE_STRAIGHT;
|
||||
break;
|
||||
}
|
||||
|
||||
/* 駆動モーター用 */
|
||||
deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
|
||||
if(deltaSpeed<0){
|
||||
motorL.speed(speed + deltaSpeed);
|
||||
motorR.speed(speed);
|
||||
}
|
||||
|
||||
/* センサーを読んで、ライントレースする */
|
||||
void trace(){
|
||||
scanSensor();
|
||||
|
||||
speed = speedPIDControl();
|
||||
speed = constrain(speed, -speedMax, speedMax);
|
||||
if(speed<0){
|
||||
motorL.speed(speedMax + speed);
|
||||
motorR.speed(speedMax);
|
||||
}else{
|
||||
motorL.speed(speed);
|
||||
motorR.speed(speed - deltaSpeed);
|
||||
motorL.speed(speedMax);
|
||||
motorR.speed(speedMax - speed);
|
||||
}
|
||||
|
||||
/* ステアリングサーボ用 */
|
||||
angle = steeringPIDControl();
|
||||
angle = constrain(angle, steeringAngleMin, steeringAngleMax);
|
||||
steering(angle);
|
||||
}
|
||||
|
||||
/* モードを決定する */
|
||||
void modeSet(){
|
||||
if(distance > lapDistance*laps || errorCount > errorCountThreshold){
|
||||
mode = MODE_STOP;
|
||||
return;
|
||||
}
|
||||
unsigned long way = distance%lapDistance;
|
||||
switch(sensor&markerSensorMask){
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -5,9 +5,8 @@
|
|||
[]---{}---[]
|
||||
/ .\
|
||||
*/
|
||||
int steering(int a){
|
||||
steeringServo.write(90 + constrain(a, steeringAngleMin, steeringAngleMax));
|
||||
return (steeringServo.read() - 90);
|
||||
void steering(int a){
|
||||
steeringServo.write(90 + a);
|
||||
}
|
||||
|
||||
/* ランスを中央からa[度]だけ動かす
|
||||
|
|
@ -16,9 +15,8 @@ int steering(int a){
|
|||
/ | \
|
||||
[]+--{}--+[]
|
||||
*/
|
||||
int lance(int a){
|
||||
void lance(int a){
|
||||
lanceServo.write(90 - a);
|
||||
return (90 - a);
|
||||
}
|
||||
|
||||
/* センサーのアレイの初期化 */
|
||||
|
|
@ -29,7 +27,7 @@ void sensorInit(){
|
|||
}
|
||||
|
||||
/* センサーアレイの状態を見る */
|
||||
unsigned char scanSensor(){
|
||||
unsigned char sensorRead(){
|
||||
unsigned char b;
|
||||
//黒が1、白が0
|
||||
for(int i=0; i<numSensors; i++){
|
||||
|
|
@ -39,6 +37,64 @@ unsigned char scanSensor(){
|
|||
return sensor = b;
|
||||
}
|
||||
|
||||
/* センサーを読んでラインの位置を返す */
|
||||
int scanSensor(){
|
||||
int pos = 0;
|
||||
switch(sensor&lineSensorMask){
|
||||
case 0b01000000:
|
||||
pos = -5;
|
||||
break;
|
||||
case 0b01100000:
|
||||
pos = -4;
|
||||
break;
|
||||
case 0b00100000:
|
||||
pos = -3;
|
||||
break;
|
||||
case 0b00110000:
|
||||
pos = -2;
|
||||
break;
|
||||
case 0b00010000:
|
||||
pos = -1;
|
||||
break;
|
||||
case 0b00011000:
|
||||
pos = 0;
|
||||
break;
|
||||
case 0b00001000:
|
||||
pos = 1;
|
||||
break;
|
||||
case 0b00001100:
|
||||
pos = 2;
|
||||
break;
|
||||
case 0b00000100:
|
||||
pos = 3;
|
||||
break;
|
||||
case 0b00000110:
|
||||
pos = 4;
|
||||
break;
|
||||
case 0b00000010:
|
||||
pos = 5;
|
||||
break;
|
||||
default:
|
||||
errorCount++;
|
||||
return 127;
|
||||
}
|
||||
errorCount = 0;
|
||||
lineDifferential = pos - linePosition;
|
||||
linePosition = pos;
|
||||
lineIntegral += pos;
|
||||
return pos;
|
||||
}
|
||||
|
||||
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
||||
int speedPIDControl(){
|
||||
return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential;
|
||||
}
|
||||
|
||||
// ステアリングの角度(度)
|
||||
int steeringPIDControl(){
|
||||
return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential;
|
||||
}
|
||||
|
||||
/* エンコーダーの割り込み処理 */
|
||||
void rot(){
|
||||
distance++;
|
||||
|
|
@ -46,7 +102,6 @@ void rot(){
|
|||
|
||||
/* 起動音 */
|
||||
void startBeep(){
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
tone(buzzerPin,2000);
|
||||
delay(100);
|
||||
tone(buzzerPin,1000);
|
||||
|
|
@ -56,7 +111,6 @@ void startBeep(){
|
|||
|
||||
/* 終了音 */
|
||||
void stopBeep(){
|
||||
pinMode(buzzerPin, OUTPUT);
|
||||
tone(buzzerPin, 2000);
|
||||
delay(2000);
|
||||
noTone(buzzerPin);
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue