2020-09-23

//---------------------------------------------------------------------------

//This Arduino sketch accompanies the OpenBot Android application.

//

//The sketch has the following functinonalities:

//- receive control commands from Android application (USB serial)

//. - produce low-level controls (PWM) for the vehicle

//- toggle left and right indicator signals

//- wheel odometry based on optical speed sensors

//- estimate battery voltage via voltage divider

//- estimate distance based on sonar sensor

//- send sensor readings to Android application (USB serial)

//

//Dependencies (if sonar is required):

//- NewPing library by Tim Eckel (Install via Tools --> Manage Libraries)

//

//By Matthias Mueller, Intelligent Systems Lab, 2020

//---------------------------------------------------------------------------

//PIN_PWM1,PIN_PWM2,PIN_PWM3,PIN_PWM4      Low-level control of left DC motors via PWM

//PIN_SPEED_L, PIN_SPEED_R                Measure left and right wheel speed

//PIN_VIN                                  Measure battery voltage via voltage divider

//PIN_TRIGGER                              Arduino pin tied to trigger pin on ultrasonic sensor.

//PIN_ECHO                                Arduino pin tied to echo pin on ultrasonic sensor.

//MAX_DISTANCE                            Maximum distance we want to ping for (in centimeters).

//PIN_LED_RL, PIN_LED_RR                  Toggle left and right rear LEDs (indicator signals)

//------------------------------------------------------//

//DEFINITIONS

//------------------------------------------------------//

#defineDIY0

#definePCB1

#defineHAS_SONAR1

//Setup the OpenBot version

#defineOPENBOTDIY

#if(OPENBOT == DIY)

#definePIN_PWM15

#definePIN_PWM26

#definePIN_PWM39

#definePIN_PWM410

#definePIN_SPEED_L2

#definePIN_SPEED_R3

#definePIN_VINA7

#definePIN_TRIGGER4

#definePIN_ECHO4

#definePIN_LED_RL7

#definePIN_LED_RR8

#elif(OPENBOT == PCB)

#definePIN_PWM110

#definePIN_PWM29

#definePIN_PWM35

#definePIN_PWM46

#definePIN_SPEED_L2

#definePIN_SPEED_R4

#definePIN_VINA7

#definePIN_TRIGGER3

#definePIN_ECHO3

#definePIN_LED_RL7

#definePIN_LED_RR8

#endif

//------------------------------------------------------//

//INITIALIZATION

//------------------------------------------------------//

#ifHAS_SONAR

//Sonar sensor

#include<NewPing.h>

constintMAX_DISTANCE =300;

NewPingsonar(PIN_TRIGGER, PIN_ECHO, MAX_DISTANCE);//NewPing setup of pins and maximum distance.

unsignedintping_interval =100;//How frequently are we going to send out a ping (in milliseconds).

unsignedlongping_timeout;//Timeout (in milliseconds). After timeout, distance is set to maximum.

unsignedlongping_time;//Holds the next ping time.

unsignedintdistance_cm = MAX_DISTANCE;

#else

#include<limits.h>

unsignedintdistance_cm = UINT_MAX;

#endif

//Vehicle Control

intctrl_left =0;

intctrl_right =0;

//Voltage measurement

unsignedintcounter_voltage =0;

constunsignedintvin_array_sz =100;

unsignedintvin_array[vin_array_sz]={0};

//Speed sensors

constunsignedlongignoremilli =0;

unsignedlongoldtime_left =0;

unsignedlongcurtime_left =0;

unsignedlongoldtime_right =0;

unsignedlongcurtime_right =0;

intcounter_left =0;

intcounter_right =0;

//Indicator Signal

constunsignedlongindicator_interval =500;

unsignedlongindicator_time =0;

intindicator_val =0;

boolindicator_left =0;

boolindicator_right =0;

boolctrl_rx =0;

boolindicator_rx =0;

//Serial communication

constunsignedlongsend_interval =1000;

unsignedlongsend_time =0;

String inString ="";

//------------------------------------------------------//

//SETUP

//------------------------------------------------------//

voidsetup()

{

//Outputs

pinMode(PIN_PWM1,OUTPUT);

pinMode(PIN_PWM2,OUTPUT);

pinMode(PIN_PWM3,OUTPUT);

pinMode(PIN_PWM4,OUTPUT);

pinMode(PIN_LED_RL,OUTPUT);

pinMode(PIN_LED_RR,OUTPUT);

//Inputs

pinMode(PIN_VIN,INPUT);

pinMode(PIN_SPEED_L,INPUT);

pinMode(PIN_SPEED_R,INPUT);

attachInterrupt(digitalPinToInterrupt(PIN_SPEED_L), speed_left, RISING);

attachInterrupt(digitalPinToInterrupt(PIN_SPEED_R), speed_right, RISING);

Serial.begin(115200,SERIAL_8N1);//8 data bits, no parity, 1 stop bit

send_time =millis() + send_interval;//wait for one interval to get readings

#ifHAS_SONAR

ping_time =millis();

#endif

}

//------------------------------------------------------//

//LOOP

//------------------------------------------------------//

voidloop() {

//Measure voltage

vin_array[counter_voltage%vin_array_sz] =analogRead(PIN_VIN);

  counter_voltage++;

#ifHAS_SONAR

//Measure distance every ping_interval

if(millis() >= ping_time) {//Ping if it's time

ping_timeout = ping_time +2* MAX_DISTANCE *10/343+1;//Set ping timeout.

ping_time += ping_interval;//Set the next ping time.

sonar.ping_timer(echoCheck);//Send out the ping, calls "echoCheck" function every 24uS where you can check the ping status.

  }

#endif


//Write voltage to serial every send_interval

if(millis() >= send_time)

  {

sendVehicleData();

    send_time += send_interval;

  }

//Check indicator signal every indicator_interval

if(millis() >= indicator_time)

  {

updateindicator();

    indicator_time += indicator_interval;

  }

if(Serial.available() >0) {

charinChar = Serial.read();

if(ctrl_rx) {

processCtrlMsg(inChar);

    }

elseif(indicator_rx) {

processindicatorMsg(inChar);

    }

else{

checkForMsg(inChar);

    }

  }

}

//------------------------------------------------------//

//FUNCTIONS

//------------------------------------------------------//

floatgetVoltage() {

unsignedlongarray_sum =0;

unsignedintarray_size =min(vin_array_sz,counter_voltage);

for(unsignedintindex=0;index< array_size;index++)

  {

array_sum += vin_array[index];

  }

returnfloat(array_sum)/array_size/1023*4.04*5;

}

voidspeed_left() {

curtime_left =millis();

if( (curtime_left - oldtime_left) > ignoremilli ) {

if(ctrl_left <0) {

      counter_left--;

    }

else{

      counter_left++;

    }

    oldtime_left = curtime_left;

  }

}

voidspeed_right() {

curtime_right =millis();

if( (curtime_right - oldtime_right) > ignoremilli ) {

if(ctrl_right <0) {

      counter_right--;

    }

else{

      counter_right++;

    }

    oldtime_right = curtime_right;

  }

}

voidprocessCtrlMsg(charinChar) {

//Serial.write(inChar);

//comma indicates that inString contains the left ctrl

if(inChar ==',') {

ctrl_left = inString.toInt();

if(ctrl_left <0) {

analogWrite(PIN_PWM1,-ctrl_left);

analogWrite(PIN_PWM2,0);

    }

elseif(ctrl_left >0) {

analogWrite(PIN_PWM1,0);

analogWrite(PIN_PWM2,ctrl_left);

    }

else{//Motor brake

analogWrite(PIN_PWM1,255);

analogWrite(PIN_PWM2,255);

    }

//clear the string for new input:

inString ="";

  }

//new line indicates that inString contains the right ctrl

elseif(inChar =='\n') {

ctrl_right = inString.toInt();//* 0.95;

if(ctrl_right <0) {

analogWrite(PIN_PWM3,-ctrl_right);

analogWrite(PIN_PWM4,0);

    }

elseif(ctrl_right >0) {

analogWrite(PIN_PWM3,0);

analogWrite(PIN_PWM4,ctrl_right);

    }

else{//Motor brake

analogWrite(PIN_PWM3,255);

analogWrite(PIN_PWM4,255);

    }

//clear the string for new input:

inString ="";

//end of message

ctrl_rx =false;

  }

else{

//As long as the incoming byte

//is not a newline or comma,

//convert the incoming byte to a char

//and add it to the string

    inString += inChar;

  }

}

voidprocessindicatorMsg(charinChar)

{

//new line indicates that inString contains the indicator signal

if(inChar =='\n') {

indicator_val = inString.toInt();

//clear the string for new input:

inString ="";

//end of message

indicator_rx =false;

  }

else{

//As long as the incoming byte

//is not a newline

//convert the incoming byte to a char

//and add it to the string

    inString += inChar;

  }

}

voidcheckForMsg(charinChar)

{

switch(inChar) {

case'c':

ctrl_rx =true;

break;

case'i':

indicator_rx =true;

break;

  }

}

voidsendVehicleData()

{

Serial.print(getVoltage());

Serial.print(",");

Serial.print(counter_left);

Serial.print(",");

Serial.print(counter_right);

Serial.print(",");

Serial.print(distance_cm);

Serial.println();

counter_left =0;

counter_right =0;

}

voidupdateindicator()

{

switch(indicator_val) {

case-1:

      indicator_left = !indicator_left;

      indicator_right = LOW;

break;

break;

case0:

      indicator_left = LOW;

      indicator_right = LOW;

break;

case1:

      indicator_left = LOW;

      indicator_right = !indicator_right;

break;

  }

digitalWrite(PIN_LED_RL, indicator_left);

digitalWrite(PIN_LED_RR, indicator_right);

}

#ifHAS_SONAR

voidechoCheck() {//Timer2 interrupt calls this function every 24uS.

if(sonar.check_timer()) {//Check ping status

distance_cm = sonar.ping_result/ US_ROUNDTRIP_CM;//Ping returned in uS, convert to cm.

  }

elseif(millis() >= ping_timeout) {

    distance_cm = MAX_DISTANCE;

  }

}

#endif

©著作权归作者所有,转载或内容合作请联系作者
  • 人面猴
    序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
    沈念sama阅读 212,686评论 6赞 492
  • 死咒
    序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
    沈念sama阅读 90,668评论 3赞 385
  • 救了他两次的神仙让他今天三更去死
    文/潘晓璐 我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
    开封第一讲书人阅读 158,160评论 0赞 348
  • 道士缉凶录：失踪的卖姜人
    文/不坏的土叔 我叫张陵，是天一观的道长。 经常有香客问我，道长，这世上最难降的妖魔是什么？ 我笑而不...
    开封第一讲书人阅读 56,736评论 1赞 284
  • 港岛之恋（遗憾婚礼）
    正文 为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
    茶点故事阅读 65,847评论 6赞 386
  • 恶毒庶女顶嫁案：这布局不是一般人想出来的
    文/花漫 我一把揭开白布。 她就那样静静地躺着，像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上，一...
    开封第一讲书人阅读 50,043评论 1赞 291
  • 城市分裂传说
    那天，我揣着相机与录音，去河边找鬼。 笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。 我是一名探鬼主播，决...
    沈念sama阅读 39,129评论 3赞 410
  • 双鸳鸯连环套：你想象不到人心有多黑
    文/苍兰香墨 我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
    开封第一讲书人阅读 37,872评论 0赞 268
  • 万荣杀人案实录
    序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
    沈念sama阅读 44,318评论 1赞 303
  • 护林员之死
    正文 独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 36,645评论 2赞 327
  • 白月光启示录
    正文 我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 38,777评论 1赞 341
  • 活死人
    序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
    沈念sama阅读 34,470评论 4赞 333
  • 日本核电站爆炸内幕
    正文 年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
    茶点故事阅读 40,126评论 3赞 317
  • 男人毒药：我在死后第九天来索命
    文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 30,861评论 0赞 21
  • 一桩弑父案，背后竟有这般阴谋
    文/苍兰香墨 我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,095评论 1赞 267
  • 情欲美人皮
    我被黑心中介骗来泰国打工， 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。 一个月前我还...
    沈念sama阅读 46,589评论 2赞 362
  • 代替公主和亲
    正文 我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
    茶点故事阅读 43,687评论 2赞 351