Arduinoを利用し、２つのプログラムを同時に動かす方法について

Question

前提・実現したいこと

アルディーノを使って、LEDテープを光らせるのとレヴィーウォークで動くロボットのプログラムを同時に行ういたい。

発生している問題・エラーメッセージ

LEDテープは光るがロボット（モーター）が動かない。

該当のソースコード

#include <avr/pgmspace.h>
#include <SoftwareSerial.h>
#include <string.h>
#include <avr/wdt.h>
#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
#include <avr/power.h> 
#include <TimedAction.h>
#endif
#define LED_PIN    10
#define LED_COUNT  13

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);


int LM1 = 7 ,LM2 = 8 ,LPWM = 6;
int RM1 = 2 ,RM2 = 4 ,RPWM = 5;
int RSSI_Read = 5;
int TS_F = 12 ,TS_B = 13;
int TOKEN =0;
int packet[100];
int address[100];
int Selection_val;
#define Motor_Out 141 // 9Vbattery
#define Motor_Out2 90 // 9Vbattery
double val_L1 = 0;//左前方センサ値:決定値
double val_R1 = 0;//右前方センサ値:決定値
double val_L2 = 0;//左後方センサ値:決定値
double val_R2 = 0;//右後方センサ値:決定値
double ave_L1 = 0;//左前方センサ値:実測値
double ave_R1 = 0;//右前方センサ値:実測値
double ave_L2 = 0;//左後方センサ値:実測値
double ave_R2 = 0;//右後方センサ値:実測値
double before=0; //行動時間決定用変数
double before2=0; //発見情報送信用変数
double before3=0;//回避角度決定用変数
int time = 500;
boolean one_time = false;
int stackL=0;//スタックカウント
int stackR=0;//同上
int advancea=0;//RW 時移動状態決定用:0=回転,1=直進
double rnd_d =0;//RW 時回転方向決定用乱数
double rnd_c=0;
double rnd_c2=0;
double rnd_f=0;//RW 時移動距離決定用乱数
double add_powerR = 0;//出力微調整用変数
double add_powerL = 0;//同上
double F_time=0;
double V_wait=0;
double V_wait2=0;
boolean direc = false;
boolean A_stackL = false;//スタック認識フラグ
boolean A_stackR = false;//同上
boolean L1_blo = false;//障害物認識フラグ
boolean R1_blo = false;//同上
boolean L2_blo = false;//同上
boolean R2_blo = false;//同上
boolean rnd_rst=false;//RW 時乱数入れ替え用フラグ
boolean rnd_rst2=false;//OA 時乱数入れ替え用フラグ
void Selection(void){
  Selection_val = random(0,2);
}

void stepper_control();
void led_control();

TimedAction motorAction = TimedAction(0,stepper_control);
TimedAction ledAction = TimedAction(1000,led_control);

void setup(){
  
#if defined(__AVR_ATtiny85__) && (F_CPU == 16000000)
  clock_prescale_set(clock_div_1);
#endif
  // END of Trinket-specific code.

  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  strip.show();            // Turn OFF all pixels ASAP
  strip.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)



// loop() function -- runs repeatedly as long as board is on ---------------

Serial.begin(115200);
pinMode(LM1,OUTPUT);
pinMode(LM2,OUTPUT);
pinMode(RM1,OUTPUT);
pinMode(RM2,OUTPUT);
pinMode(LPWM,OUTPUT);
pinMode(RPWM,OUTPUT);
pinMode(TS_F,INPUT);
pinMode(TS_B,INPUT);
pinMode(RSSI_Read,INPUT);
randomSeed(analogRead(4));
wdt_enable(WDTO_1S);
}

void loop(){
  motorAction.check();
  ledAction.check();
}

void stepper_control(){
   Receive();
  if(packet[17]==0xAA){
    add_powerR=packet[15] - Motor_Out;
    add_powerL=packet[16] - Motor_Out;
}


  if(packet[15]==0x47){//GO 信号受け取り
  while(1){
    time=500;
    Obstacle_Sensor();
    wdt_reset();//これが一定時間(1s)実行されなければリセットされる
  if (Target_Discovery() == 0){//ボールを見つけたら 1,それ以外は 0
    Obstacle_avoidance();
    }
    else{
      Receive();//受信関数
  if(packet[15]==0x53){//STOP 信号受け取り
    delay(1);
    TOKEN=0;
    break;
    }
    Motor_Drive('S');//ボールを発見したら停車
    }
    Receive();//受信関数
  if(packet[15]==0x53){//STOP 信号受け取り
    delay(1);
    break;
    }
    }
    }
    else{
      wdt_reset();//これが一定時間(1s)実行されなければリセットされる
      Motor_Drive('S');
      delay(1);
      return;
}
}
void led_control(){
  colorWipe(strip.Color(255,   0,   0), 25); // Red
  colorWipe(strip.Color(  0, 255,   0), 25); // Green
  colorWipe(strip.Color(  0,   0, 255), 25); // Blue
}
  

void colorWipe(uint32_t color, int wait) {
  for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
    strip.setPixelColor(i, color);         //  Set pixel's color (in RAM)
    strip.show();                          //  Update strip to match
    delay(wait);                           //  Pause for a moment
  }
}
void Motor_Drive(int a)
  {
  if ( a == 'F')
  {
//前進
  digitalWrite(LM1,HIGH);
  digitalWrite(LM2,LOW);
  digitalWrite(RM1,HIGH);
  digitalWrite(RM2,LOW);
}
  else if ( a == 'B' )
  {
//後進
  digitalWrite(LM1,LOW);
  digitalWrite(LM2,HIGH);
  digitalWrite(RM1,LOW);
  digitalWrite(RM2,HIGH);
}
  else if ( a == 'S')
  {
  digitalWrite(LM1,HIGH);
  digitalWrite(LM2,HIGH);
  digitalWrite(RM1,HIGH);
  digitalWrite(RM2,HIGH);
}
  else if ( a == 'L' )
  {
  digitalWrite(LM1,LOW);
  digitalWrite(LM2,HIGH);
  digitalWrite(RM1,HIGH);
  digitalWrite(RM2,LOW);
}
  else if ( a == 'R' )
{
  digitalWrite(LM1,HIGH);
  digitalWrite(LM2,LOW);
  digitalWrite(RM1,LOW);
  digitalWrite(RM2,HIGH);
}
}
  void Motor_Power(int a){
    int b,c;
    if(advancea==1){
    b=a+add_powerR;
    c=a+add_powerL;
}
  else{
    b=a;
    c=a;
}
  b=constrain(b,70,254);
  c=constrain(c,70,254);
  analogWrite(RPWM, b);
  analogWrite(LPWM, c);
}
int Random_value(int Dom){//ランダム値決定用
int a =random(10000);
Dom=a%Dom;
return (Dom);
}
int wait_sec(int waitsec) {//移動時間倍数決定
if (waitsec >= 0 && waitsec < 3259) return(1) ; //32.59%
if (waitsec >= 3259 && waitsec < 4677) return(2) ; //14.18%
if (waitsec >= 4677 && waitsec < 5549) return(3) ; //8.72%
if (waitsec >= 5549 && waitsec < 6166) return(4) ; //6.17%
if (waitsec >= 6166 && waitsec < 6638) return(5) ; //4.72%
if (waitsec >= 6638 && waitsec < 7017) return(6) ; //3.79%
if (waitsec >= 7017 && waitsec < 7332) return(7) ; //3.15%
if (waitsec >= 7332 && waitsec < 7600) return(8) ; //2.68%
if (waitsec >= 7600 && waitsec < 7833) return(9) ; //2.33%
if (waitsec >= 7833 && waitsec < 8038) return(10) ; //2.05%
if (waitsec >= 8038 && waitsec < 8221) return(11) ; //1.83%
if (waitsec >= 8221 && waitsec < 8386) return(12) ; //1.65%
if (waitsec >= 8386 && waitsec < 8536) return(13) ; //1.50%
if (waitsec >= 8536 && waitsec < 8673) return(14) ; //1.37%
if (waitsec >= 8673 && waitsec < 8799) return(15) ; //1.26%
if (waitsec >= 8799 && waitsec < 8916) return(16) ; //1.17%
if (waitsec >= 8916 && waitsec < 9024) return(17) ; //1.08%
if (waitsec >= 9024 && waitsec < 9125) return(18) ; //1.01%
if (waitsec >= 9125 && waitsec < 9220) return(19) ; //0.95%
if (waitsec >= 9220 && waitsec < 9309) return(20) ; //0.89%
if (waitsec >= 9309 && waitsec < 9393) return(21) ; //0.84%
if (waitsec >= 9393 && waitsec < 9472) return(22) ; //0.79%
if (waitsec >= 9472 && waitsec < 9547) return(23) ; //0.75%
if (waitsec >= 9547 && waitsec < 9618) return(24) ; //0.71%
if (waitsec >= 9618 && waitsec < 9686) return(25) ; //0.68%
if (waitsec >= 9686 && waitsec < 9751) return(26) ; //0.65%
if (waitsec >= 9751 && waitsec < 9813) return(27) ; //0.62%
if (waitsec >= 9813 && waitsec < 9872) return(28) ; //0.59%
if (waitsec >= 9872 && waitsec < 9929) return(29) ; //0.57%
if (waitsec >= 9929) return(30) ; //0.55%
return(-1) ;
}
void Random_Walk(int c)//
{
const int wait = c; //30 回転
const int wait2 = 6000;//移動時間基準値
double a,b;
if(rnd_rst==false){//ランダム値決定
rnd_f =Random_value(9999); //移動距離決定
rnd_d =Random_value(2); //回転方向決定
rnd_c =Random_value(3)+1; //回転角度決定
rnd_rst =true;
a=wait_sec(rnd_f);
F_time =wait2 * a ;//
V_wait = wait * rnd_c;//deg 30 60 90
}
if(millis() - before <=V_wait && advancea==0 && rnd_d == 0){//左 30 回転
Motor_Drive('L');
Motor_Power(Motor_Out2);
}
else if(millis() - before <=V_wait + 2000 && advancea==0 && rnd_d ==0){//前進時

advancea=1;
before=millis();
rnd_rst =false;
}
else if(millis() - before <=V_wait && advancea==0 && rnd_d ==1){//右 30 回転
Motor_Drive('R');
Motor_Power(Motor_Out2);
}
else if(millis() - before <=V_wait + 2000 && advancea==0 && rnd_d ==1){//前進時
advancea=1;
before=millis();
rnd_rst =false;
}
//前進
else if(millis() - before <= F_time && advancea==1){ //直進:障害物軽回避
stackL=0;
stackR=0;
direc=false;
Motor_Drive('F');
Motor_Power(Motor_Out);
}
//後進
else if(millis() - before <= F_time && advancea==2){
stackL=0;
stackR=0;
direc=true;
Motor_Drive('B');
Motor_Power(Motor_Out);
}
//初期化&同期
else{
advancea=0;
before=millis();
stackL = 0;
stackR = 0;
rnd_rst=false;
}
return;
}
void No_Delay_Avoid(){
const int wait = 400;//回転時間基準値
if(rnd_rst2==false){//ランダム値決定
before3=millis();
rnd_rst2 =true;
}
if(L1_blo==true){//左前方障害物フラグがオンのとき実行
if(millis()-before3<=wait){
Motor_Drive('R');
Motor_Power(Motor_Out2);
}
else{
before=before+wait;
L1_blo=false;
rnd_rst2 =false;
return;
}
}
else if(R1_blo==true){
if(millis()-before3<=wait){
Motor_Drive('L');
Motor_Power(Motor_Out2);
}
else{
before=before+wait;
R1_blo=false;
rnd_rst2 =false;
return;
}
}
else if(L2_blo==true){
if(millis()-before3<=wait*0.5){
Motor_Drive('R');
Motor_Power(Motor_Out2);
}
else{
before=before+wait;
L2_blo=false;
rnd_rst2 =false;
return;
}
}
else{
if(millis()-before3<=wait*0.5){
Motor_Drive('L');
Motor_Power(Motor_Out2);
}
else{
before=before+wait;
R2_blo=false;
rnd_rst2 =false;
return;
}
}
return;
}

・障害物回避のプログラム
・ターゲット探索のプログラム

//シリアルモニタ用プログラム
if (packet[0] == 0x7e){
  for ( int i = 0 ; i < 30 ; i++ ){
    delay(1);
}
}
else{
  delay(1);
}
return 0;
}//受信関数ここまで。

試したこと

レヴィーウォークで動くロボットのプログラムにLEDテープを光らせるプログラムを書き込み2つのプログラムを動かす（TimedAction）を追加したのですがLEDが光るだけでロボットは動かない。逆にロボットが動いた場合はLEDが光らなくなる。

Comments

[thkana]

どういう基準で掲載プログラムを削ったのかわからないけれど、少なくともこれではプログラムの動作を追えません。
それと、他の環境に移植するつもりがないのなら、無用な#ifdefは削った方がいいです。読みにくい。

[thkana]

https://detail.chiebukuro.yahoo.co.jp/qa/question_detail/q12254387707
https://teratail.com/help#posted-otherservice を見て、適宜対応してください。

Answer 1

早く全文が出ないか待っているんだが・・・

このスケッチはLEDテープの部分無しでちゃんとロボットは動くのですか？

RPWM = 5;

RSSI_Read = 5;

pinMode(RPWM,OUTPUT);

pinMode(RSSI_Read,INPUT);

このあたりにも原因がありそうだけど、RSSI_Readがゴミなのか使っているのかが判断出来ない。
因みにAVRマイコンならdigitalReadすれば、PWMは停止させる筈。

それと私見だが、そんなちまいライブラリなんか使わずにそのまま

void loop(){
	static uint32_t time;
	stepper_control();
	if(millis()-time>=1000){
		time=millis();
		led_control();
	}
}

のように自分で書いた方が良い。何でこの程度の事でライブラリを使ったのか？
(ライブラリを理解しないまま使うのは一番良くない)

更に言えば、ステッピングモータを使っていないのにstepper_controlの名前も如何なものか？