Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#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