Created
April 3, 2014 21:51
-
-
Save takahi5/9963634 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <Distance.h> | |
#define RUN_MODE_STOP 0 | |
#define RUN_MODE_RUN 1 | |
#define MOTOR_MODE_STOP 0 | |
#define MOTOR_MODE_FORWARD 1 | |
#define MOTOR_MODE_BACKWARD 2 | |
#define MOTOR_MODE_RIGHT 3 | |
#define MOTOR_MODE_LEFT 4 | |
#define MOTOR_MODE_BREAK 5 | |
#define CLEAN_MODE_RANDAM 0 | |
#define CLEAN_MODE_CORNER 1 | |
#define CLEAN_MODE_BOTH 3 | |
#define OUT_MOTOR1_A 4 | |
#define OUT_MOTOR1_B 5 | |
#define OUT_MOTOR1_PWM 3 | |
#define OUT_MOTOR2_A 12 | |
#define OUT_MOTOR2_B 13 | |
#define OUT_MOTOR2_PWM 11 | |
#define OUT_MOTOR_BRUSH_A 8 | |
#define OUT_MOTOR_BRUSH_PWM 9 | |
#define OUT_LED 10 | |
#define IN_TOUCH_SENSOR 7 | |
#define IN_BUTTON_MODE 6 | |
#define IN_BUTTON_START 2 | |
#define WINDUP_GUARD_GAIN 100.0 | |
#define PID_UPDATE_INTERVAL 200 // milliseconds | |
#define PGAIN_ADR 0 | |
#define IGAIN_ADR 4 | |
#define DGAIN_ADR 8 | |
int delayTime; | |
int presetMode; | |
int oldTouchSensorVal = 0; | |
int oldModeButtonVal = 0; | |
int oldStartButtonVal = 0; | |
int currentCleanMode = 0; | |
int currentMilliTime = 0; | |
int currentMotorMode; | |
int currentRunMode; | |
//time_t prevtime; | |
int startSec; | |
//---PID--- | |
float iState = 0; | |
float lastDistance = 0; | |
float pgain; | |
float igain; | |
float dgain; | |
float pTerm, iTerm, dTerm; | |
int pgainAddress, igainAddress, dgainAddress; | |
unsigned long lastPIDTime; // most recent PID update time in ms | |
int targetDistance; | |
Distance myDistance = Distance(4); | |
void setup(){ | |
pinMode(OUT_MOTOR1_A,OUTPUT); //信号用ピン | |
pinMode(OUT_MOTOR1_B,OUTPUT); //信号用ピン | |
pinMode(OUT_MOTOR2_A,OUTPUT); //信号用ピン | |
pinMode(OUT_MOTOR2_B,OUTPUT); //信号用ピン | |
pinMode(OUT_MOTOR_BRUSH_A,OUTPUT); //信号用ピン | |
pinMode(OUT_LED, OUTPUT); | |
pinMode(IN_TOUCH_SENSOR,INPUT); //入力用ピン | |
pinMode(IN_BUTTON_MODE,INPUT); //入力用ピン | |
pinMode(IN_BUTTON_START,INPUT); //入力用ピン | |
Serial.begin(9600); // シリアルポートを9600bpsで開く | |
setP(30.0); // make sure to keep the decimal point on these values | |
setI(0.0); // make sure to keep the decimal point on these values | |
setD(0.0); // make sure to keep the decimal point on these values | |
lastPIDTime = millis(); | |
targetDistance = 130;//100mm target | |
delayTime = 1000; | |
currentRunMode = RUN_MODE_STOP; | |
} | |
void loop(){ | |
//赤外線距離センサ | |
myDistance.available(); | |
currentMilliTime = millis(); | |
blinkLed(currentMilliTime);//LEDの点灯 | |
//=================STOP================================ | |
if(currentRunMode == RUN_MODE_STOP){ | |
//Motor | |
setMotorMode(MOTOR_MODE_STOP); | |
//Mode Button | |
if(isModeButtonPressed() == true){//Button up | |
if(currentCleanMode == CLEAN_MODE_RANDAM)currentCleanMode = CLEAN_MODE_CORNER; | |
else if(currentCleanMode == CLEAN_MODE_CORNER)currentCleanMode = CLEAN_MODE_BOTH; | |
else currentCleanMode = CLEAN_MODE_RANDAM; | |
} | |
//Start Button | |
if(isStartButtonPressed() == true){ | |
currentRunMode = RUN_MODE_RUN; | |
} | |
} | |
else{//=================RUN================================ | |
if(currentCleanMode == CLEAN_MODE_CORNER){//壁際掃除モード | |
if(isTouchSensorPressed() == true){ | |
motorBackAndTurnLeft(); | |
} | |
//PID制御 | |
// This checks for rollover with millis() | |
if (currentMilliTime < lastPIDTime) { | |
lastPIDTime = 0; | |
} | |
if ((currentMilliTime - lastPIDTime) > PID_UPDATE_INTERVAL) { | |
lastPIDTime += PID_UPDATE_INTERVAL; | |
int dist = myDistance.numberOfDistance(); | |
int input = updatePID(targetDistance, dist); | |
setMotorRotation(input); | |
} | |
} | |
else{//ランダム反転掃除モード | |
if(isTouchSensorPressed() == true){ | |
motorBackAndTurn(); | |
} | |
else{ | |
setMotorMode(MOTOR_MODE_FORWARD); | |
} | |
setBrushMotorMode(MOTOR_MODE_FORWARD); | |
} | |
if(isStartButtonPressed() == true){ | |
currentRunMode = RUN_MODE_STOP; | |
setMotorMode(MOTOR_MODE_STOP); | |
setBrushMotorMode(MOTOR_MODE_STOP); | |
} | |
} | |
} | |
//LEDの点滅 | |
// ランダム掃除モード:遅い点滅 | |
// 壁際掃除モード :速い点滅 | |
void blinkLed(int currentMilliTime){ | |
if(currentCleanMode == CLEAN_MODE_RANDAM){ | |
if(abs(currentMilliTime % 1000) < 500) digitalWrite(OUT_LED,HIGH); | |
else digitalWrite(OUT_LED,LOW); | |
} | |
else if(currentCleanMode == CLEAN_MODE_CORNER){ | |
if(abs(currentMilliTime % 500) < 250) digitalWrite(OUT_LED,HIGH); | |
else digitalWrite(OUT_LED,LOW); | |
} | |
else{//CLEAN_MODE_BOTH | |
digitalWrite(OUT_LED,HIGH); | |
} | |
} | |
//モード変更ボタンが押されたときの処理 | |
boolean isModeButtonPressed(){ | |
//BUTTON | |
boolean ret = false; | |
int modeButtonVal = digitalRead(IN_BUTTON_MODE); | |
if((modeButtonVal == LOW) && (oldModeButtonVal == HIGH)){//Button up | |
ret = true; | |
delay(50);//チャタリング対策 | |
} | |
oldModeButtonVal = modeButtonVal; | |
return ret; | |
} | |
//スタートボタンが押されたときの処理 | |
boolean isStartButtonPressed(){ | |
//BUTTON | |
boolean ret = false; | |
int startButtonVal = digitalRead(IN_BUTTON_START); | |
if((startButtonVal == LOW) && (oldStartButtonVal == HIGH)){//Button up | |
ret = true; | |
delay(50);//チャタリング対策 | |
} | |
oldStartButtonVal = startButtonVal; | |
return ret; | |
} | |
//前方の衝突検知用のタッチセンサが反応したときの処理 | |
boolean isTouchSensorPressed(){ | |
//BUTTON | |
boolean ret = false; | |
int touchSensorVal = digitalRead(IN_TOUCH_SENSOR); | |
if((touchSensorVal == HIGH) && (oldTouchSensorVal == LOW)){//Button up | |
ret = true; | |
delay(50);//チャタリング対策 | |
} | |
oldTouchSensorVal = touchSensorVal; | |
return ret; | |
} | |
//掃除ブラシのモータ制御 | |
void setBrushMotorMode(int mode) | |
{ | |
switch(mode){ | |
case MOTOR_MODE_STOP: | |
digitalWrite(OUT_MOTOR_BRUSH_A,LOW); | |
analogWrite(OUT_MOTOR_BRUSH_PWM,0); | |
break; | |
case MOTOR_MODE_FORWARD: | |
digitalWrite(OUT_MOTOR_BRUSH_A,HIGH); | |
analogWrite(OUT_MOTOR_BRUSH_PWM,200); | |
break; | |
} | |
} | |
//走行用モータの制御 | |
void setMotorMode(int mode){ | |
if(mode == currentMotorMode)return; | |
currentMotorMode = mode; | |
switch(mode){ | |
case MOTOR_MODE_BREAK: | |
digitalWrite(OUT_MOTOR1_A,HIGH); | |
digitalWrite(OUT_MOTOR1_B,HIGH); | |
digitalWrite(OUT_MOTOR2_A,HIGH); | |
digitalWrite(OUT_MOTOR2_B,HIGH); | |
analogWrite(OUT_MOTOR1_PWM,0); | |
analogWrite(OUT_MOTOR2_PWM,0); | |
break; | |
case MOTOR_MODE_STOP: | |
digitalWrite(OUT_MOTOR1_A,LOW); | |
digitalWrite(OUT_MOTOR1_B,LOW); | |
digitalWrite(OUT_MOTOR2_A,LOW); | |
digitalWrite(OUT_MOTOR2_B,LOW); | |
analogWrite(OUT_MOTOR1_PWM,0); | |
analogWrite(OUT_MOTOR2_PWM,0); | |
break; | |
case MOTOR_MODE_BACKWARD: | |
digitalWrite(OUT_MOTOR1_A,HIGH); | |
digitalWrite(OUT_MOTOR1_B,LOW); | |
digitalWrite(OUT_MOTOR2_A,HIGH); | |
digitalWrite(OUT_MOTOR2_B,LOW); | |
analogWrite(OUT_MOTOR1_PWM,200); | |
analogWrite(OUT_MOTOR2_PWM,200); | |
break; | |
case MOTOR_MODE_FORWARD: | |
digitalWrite(OUT_MOTOR1_A,LOW); | |
digitalWrite(OUT_MOTOR1_B,HIGH); | |
digitalWrite(OUT_MOTOR2_A,LOW); | |
digitalWrite(OUT_MOTOR2_B,HIGH); | |
analogWrite(OUT_MOTOR1_PWM,200); | |
analogWrite(OUT_MOTOR2_PWM,200); | |
break; | |
case MOTOR_MODE_LEFT: | |
digitalWrite(OUT_MOTOR1_A,HIGH); | |
digitalWrite(OUT_MOTOR1_B,LOW); | |
digitalWrite(OUT_MOTOR2_A,LOW); | |
digitalWrite(OUT_MOTOR2_B,HIGH); | |
analogWrite(OUT_MOTOR1_PWM,200); | |
analogWrite(OUT_MOTOR2_PWM,200); | |
break; | |
case MOTOR_MODE_RIGHT: | |
digitalWrite(OUT_MOTOR1_A,LOW); | |
digitalWrite(OUT_MOTOR1_B,HIGH); | |
digitalWrite(OUT_MOTOR2_A,HIGH); | |
digitalWrite(OUT_MOTOR2_B,LOW); | |
analogWrite(OUT_MOTOR1_PWM,200); | |
analogWrite(OUT_MOTOR2_PWM,200); | |
break; | |
} | |
} | |
//少し後退して反転する処理 | |
void motorBackAndTurn(){ | |
int rand = random(1, 6) ; | |
int rotationTime = rand * 500;//500~3000msec | |
setMotorMode(MOTOR_MODE_BREAK); | |
delay(500); | |
setMotorMode(MOTOR_MODE_BACKWARD); | |
delay(1000); | |
setMotorMode(MOTOR_MODE_BREAK); | |
delay(500); | |
setMotorMode(MOTOR_MODE_RIGHT); | |
delay(rotationTime); | |
//setMotorMode(MOTOR_MODE_STOP); | |
setMotorMode(MOTOR_MODE_FORWARD); | |
} | |
//少し後退して左に反転する処理 | |
void motorBackAndTurnLeft(){ | |
setMotorMode(MOTOR_MODE_BREAK); | |
delay(500); | |
setMotorMode(MOTOR_MODE_BACKWARD); | |
delay(500); | |
setMotorMode(MOTOR_MODE_BREAK); | |
delay(500); | |
setMotorMode(MOTOR_MODE_LEFT); | |
delay(500); | |
setMotorMode(MOTOR_MODE_FORWARD); | |
} | |
//壁際走行中のモータ出力制御 | |
//入力は -255 ~ 255 | |
void setMotorRotation(int input){ | |
if(input > 1000)input = 1000; | |
else if(input < (-1000))input = (-1000); | |
input = input / 4;//input is -255 to 255 | |
int pwmInput2 = (input / 4) + 128;//0to255 | |
int pwmInput1 = 255 - pwmInput2;//0to2555 | |
digitalWrite(OUT_MOTOR1_A,LOW); | |
digitalWrite(OUT_MOTOR1_B,HIGH); | |
digitalWrite(OUT_MOTOR2_A,LOW); | |
digitalWrite(OUT_MOTOR2_B,HIGH); | |
analogWrite(OUT_MOTOR1_PWM,pwmInput1); | |
analogWrite(OUT_MOTOR2_PWM,pwmInput2); | |
Serial.print("$distance="); | |
Serial.print(lastDistance); | |
Serial.print("$motor_r="); | |
Serial.print(pwmInput1); | |
Serial.print("$motor_l="); | |
Serial.println(pwmInput2); | |
} | |
//===================PID=========================== | |
void setupPID(unsigned int padd, int iadd, int dadd) { | |
// with this setup, you pass the addresses for the PID algorithm to use to | |
// for storing the gain settings. This way wastes 6 bytes to store the addresses, | |
// but its nice because you can keep all the EEPROM address allocaton in once place. | |
pgainAddress = padd; | |
igainAddress = iadd; | |
dgainAddress = dadd; | |
// pgain = readFloat(pgainAddress); | |
// igain = readFloat(igainAddress); | |
// dgain = readFloat(dgainAddress); | |
} | |
float getP() { | |
// get the P gain | |
return pgain; | |
} | |
float getI() { | |
// get the I gain | |
return igain; | |
} | |
float getD() { | |
// get the D gain | |
return dgain; | |
} | |
void setP(float p) { | |
// set the P gain and store it to eeprom | |
pgain = p; | |
//writeFloat(p, pgainAddress); | |
} | |
void setI(float i) { | |
// set the I gain and store it to eeprom | |
igain = i; | |
//writeFloat(i, igainAddress); | |
} | |
void setD(float d) { | |
// set the D gain and store it to eeprom | |
dgain = d; | |
// writeFloat(d, dgainAddress); | |
} | |
float updatePID(float targetDistance, float curDistance) | |
{ | |
// these local variables can be factored out if memory is an issue, | |
// but they make it more readable | |
double result; | |
float error; | |
float windupGaurd; | |
// determine how badly we are doing | |
error = targetDistance - curDistance; | |
// the pTerm is the view from now, the pgain judges | |
// how much we care about error we are this instant. | |
pTerm = pgain * error; | |
// iState keeps changing over time; it's | |
// overall "performance" over time, or accumulated error | |
iState += error; | |
// to prevent the iTerm getting huge despite lots of | |
// error, we use a "windup guard" | |
// (this happens when the machine is first turned on and | |
// it cant help be cold despite its best efforts) | |
// not necessary, but this makes windup guard values | |
// relative to the current iGain | |
windupGaurd = WINDUP_GUARD_GAIN / igain; | |
if (iState > windupGaurd) | |
iState = windupGaurd; | |
else if (iState < -windupGaurd) | |
iState = -windupGaurd; | |
iTerm = igain * iState; | |
// the dTerm, the difference between the temperature now | |
// and our last reading, indicated the "speed," | |
// how quickly the temp is changing. (aka. Differential) | |
dTerm = (dgain* (curDistance - lastDistance)); | |
// now that we've use lastDistance, put the current temp in | |
// our pocket until for the next round | |
lastDistance = curDistance; | |
float ret = pTerm + iTerm - dTerm; | |
// the magic feedback bit | |
return ret; | |
} | |
void printPIDDebugString() { | |
// // A helper function to keep track of the PID algorithm | |
// Serial.print("PID formula (P + I - D): "); | |
// | |
// printFloat(pTerm, 2); | |
// Serial.print(" + "); | |
// printFloat(iTerm, 2); | |
// Serial.print(" - "); | |
// printFloat(dTerm, 2); | |
// Serial.print(" POWER: "); | |
// printFloat(getHeatCycles(), 0); | |
// Serial.print(" "); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment