Processing模拟控制多台舵机-以距离为参数程序参考

2024-01-09 08:07:10

又是一次课程学习的结束，辅导学生的过程也很受益，温故而知新。该组同学需要Arduino控制多达6个舵机，而且基于距离这一参数，在不同距离值之间会有不同的触发事件，也就是6个舵机转的角度都有所不同，而且还伴随着几盏灯的变化。灯的变化我在这篇文章中就略去以后谈，就以控制舵机来做一总结。我首先用Processing来模拟这一过程。因为Processing可以借Arduino(Firmata)库来实现P5代理Arduino运作，后期可直接使用该模型控制舵机，倘若不用该库，直接移植到C/C++ 语言系统中编写Arduino程序执行运作也非常方便。因此模拟的这一举措是值得参考的。

开始

首先要规划好每个状态，实质上这有点创建状态机的味道，但是因为这是学生想要做的交互模型，这次就从简了。不同区间所触发的事件见下表所示:

当然表格中的数值是我想当然的，暂且就这么定。
【1】分了5块区域，分别去检测距离值distance是否在该区域，作相应动作
【2】一种6个舵机 ---- 上方3个 UP-A UP-B UP-C 下方3个 DOWN-A DOWN-B DOWN-C

编写

变量声明

/////////////////////////////
//  6个舵机 信号引脚 定义
/////////////////////////////
final int UP_A_SERVO = 11;
final int UP_B_SERVO = 10;
final int UP_C_SERVO = 9;
final int DOWN_A_SERVO = 6;
final int DOWN_B_SERVO = 5;
final int DOWN_C_SERVO = 3;

///////////////////////////////
//  6个舵机 实际角度 声明定义
///////////////////////////////
float UP_A_SERVO_rot = 0;
float UP_B_SERVO_rot = 0;
float UP_C_SERVO_rot = 0;
float DOWN_A_SERVO_rot = 0;
float DOWN_B_SERVO_rot = 0;
float DOWN_C_SERVO_rot = 0;

///////////////////////////////
//  6个舵机 目标角度 声明定义
///////////////////////////////
float UP_A_SERVO_rot_target = 0;
float UP_B_SERVO_rot_target = 0;
float UP_C_SERVO_rot_target = 0;
float DOWN_A_SERVO_rot_target = 0;
float DOWN_B_SERVO_rot_target = 0;
float DOWN_C_SERVO_rot_target = 0;

//  检测的距离  待输入
int distance = 0;
//  缓动系数，控制数值变化快慢
float easing = 0.02;

//////////////////////////////////////
//  方便计算每帧流逝的时间，方便计时
//////////////////////////////////////
float nowtime;
float oldtime;
float deltatime;
float timer; // 计时器，具体数值代表总流逝的时间

//   状态量   距离数值的区间
//*******************
//   0  缺省
//   1  区间1
//   2  区间2
//   3  区间3
//   4  区间4
//   5  区间5
//*******************
int state = 0;

//   计时器触发次数的计数
int tempindex = 0;

//   方便检测区间变动的触发[事件]
int frame4state1 = 0;
int frame4state2 = 0;
int frame4state3 = 0;
int frame4state4 = 0;
int frame4state5 = 0;

//   声明一个改变数值变化模式的开关量
//   false  -  线性变化
//   true   -  缓动变化
//   待完善 可改为枚举，创建多种方式
boolean valueChangeKey = false;
//////////////////////////////////////
//  定义每区间极值 最小 最大
//////////////////////////////////////
final static int DurA1 = 0;
final static int DurB1 = 50;

final static int DurA2 = 50;
final static int DurB2 = 100;

final static int DurA3 = 100;
final static int DurB3 = 200;

final static int DurA4 = 200;
final static int DurB4 = 400;

final static int DurA5 = 400;
final static int DurB5 = 500;

///////////////////////////////////////////////
//  临时变量 表示代表更换区间展示的UI方框透明度
///////////////////////////////////////////////
float value4durChange_1 = 0;
float value4durChange_2 = 0;
float value4durChange_3 = 0;
float value4durChange_4 = 0;
float value4durChange_5 = 0;

//////////////////////////////////////
//  临时变量 表示每舵机变化时的状态
//////////////////////////////////////
float value4ServoState_1 = 0;
float value4ServoState_2 = 0;
float value4ServoState_3 = 0;
float value4ServoState_4 = 0;
float value4ServoState_5 = 0;
float value4ServoState_6 = 0;

////////////////////////////////////////////
//  临时变量 表示每舵机可视化UI的方框透明度
////////////////////////////////////////////
float value4ServoChange_1 = 0;
float value4ServoChange_2 = 0;
float value4ServoChange_3 = 0;
float value4ServoChange_4 = 0;
float value4ServoChange_5 = 0;
float value4ServoChange_6 = 0;


////////////////////////////////////////////////////
//  临时变量 表示每舵机更改目标参数时的触发事件开关
////////////////////////////////////////////////////
boolean temp4changekey_1 = false;
boolean temp4changekey_2 = false;
boolean temp4changekey_3 = false;
boolean temp4changekey_4 = false;
boolean temp4changekey_5 = false;
boolean temp4changekey_6 = false;

重要函数

// 线性数值变化
float getLinearValue(float x, float tx)
{
  if (x < tx)
    x += 1;
  else if (x > tx)
    x -= 1;
  else
    x = tx;
    
  return x;
}

// 更新状态
void updateState() {
  if (state == 0) {
    frame4state1 = 0;
    frame4state2 = 0;
    frame4state3 = 0;
    frame4state4 = 0;
    frame4state5 = 0;
  }
  if (state == 1) {
    //frame4state1 = 0;
    frame4state2 = 0;
    frame4state3 = 0;
    frame4state4 = 0;
    frame4state5 = 0;
  }
  if (state == 2) {
    frame4state1 = 0;
    //frame4state2 = 0;
    frame4state3 = 0;
    frame4state4 = 0;
    frame4state5 = 0;
  }
  if (state == 3) {
    frame4state1 = 0;
    frame4state2 = 0;
    //frame4state3 = 0;
    frame4state4 = 0;
    frame4state5 = 0;
  }
  if (state == 4) {
    frame4state1 = 0;
    frame4state2 = 0;
    frame4state3 = 0;
    //frame4state4 = 0;
    frame4state5 = 0;
  }

  if (state == 5) {
    frame4state1 = 0;
    frame4state2 = 0;
    frame4state3 = 0;
    frame4state4 = 0;
    //frame4state5 = 0;
  }
}

// 处理状态
void handleState() {

  if (state == 2)
  {
    timer += deltatime;
    if (timer > 2000)
    {
      timer = 0;
      tempindex ++;

      if (tempindex == 1) {
        DOWN_B_SERVO_rot_target = 90;
        value4ServoState_5 = 5;  // 既是触发状态,又是计数5[可看成5帧]，下同
      }

      if (tempindex == 2) {
        DOWN_C_SERVO_rot_target = 90;
        value4ServoState_6 = 5;
      }
    }
  }


  if (state == 3)
  {
    timer += deltatime;
    if (timer > 2000)
    {
      timer = 0;
      tempindex ++;

      if (tempindex == 1) {
        DOWN_A_SERVO_rot_target = 90;
        value4ServoState_4 = 5;
      }

      if (tempindex == 2) {
        DOWN_B_SERVO_rot_target = 90;
        value4ServoState_5 = 5;
      }

      if (tempindex == 3) {
        DOWN_C_SERVO_rot_target = 90;
        value4ServoState_6 = 5;
      }
    }
  }


  if (state == 4)
  {
    timer += deltatime;
    if (timer > 2000)
    {
      timer = 0;
      tempindex ++;

      if (tempindex == 1) {
        DOWN_B_SERVO_rot_target = 0;
        value4ServoState_5 = 5;
      }

      if (tempindex == 2) {
        DOWN_A_SERVO_rot_target = 0;
        value4ServoState_4 = 5;
      }
    }
  }
}

// 更新每区间
void updatePerDuring() {
  //*************
  // DUR 1
  //*************

  if (distance >= DurA1 && distance < DurB1)
  {
    if (frame4state1 == 0) {   //这里的if判断结构代表区间变化所触发的事件[执行一次]，下同
      tempindex = 0;
      timer = 0;
      value4durChange_1 = 250;

      value4ServoState_1 = 5;
      value4ServoState_2 = 5;
      value4ServoState_3 = 5;
      value4ServoState_4 = 5;
      value4ServoState_5 = 5;
      value4ServoState_6 = 5;
    }
    frame4state1 ++;
    state = 1;

    UP_A_SERVO_rot_target = 30;
    UP_B_SERVO_rot_target = 90;
    UP_C_SERVO_rot_target = 150;
    DOWN_A_SERVO_rot_target = 0;
    DOWN_B_SERVO_rot_target = 0;
    DOWN_C_SERVO_rot_target = 0;
  }



  //*************
  // DUR 2
  //*************

  if (distance >= DurA2 && distance < DurB2)
  {
    if (frame4state2 == 0) {
      tempindex = 0;
      timer = 0;
      value4durChange_2 = 250;
      value4ServoState_1 = 5;
      value4ServoState_2 = 5;
      value4ServoState_3 = 5;
      value4ServoState_4 = 5;
    }
    frame4state2 ++;

    state = 2;

    UP_A_SERVO_rot_target = 50;
    UP_B_SERVO_rot_target = 80;
    UP_C_SERVO_rot_target = 120;

    if (tempindex == 0) {
      DOWN_A_SERVO_rot_target = 90;
    }
  }


  //*************
  // DUR 3
  //*************

  if (distance >= DurA3 && distance < DurB3) {
    if (frame4state3 == 0) {
      tempindex = 0;
      timer = 0;
      value4durChange_3 = 250;

      value4ServoState_1 = 5;
      value4ServoState_2 = 5;
      value4ServoState_3 = 5;
    }
    frame4state3 ++;

    state = 3;

    UP_A_SERVO_rot_target = 0;
    UP_B_SERVO_rot_target = 0;
    UP_C_SERVO_rot_target = 0;
  }


  //*************
  // DUR 4
  //*************

  if (distance >= DurA4 && distance < DurB4) {
    if (frame4state4 == 0) {
      tempindex = 0;
      timer = 0;
      value4durChange_4 = 250;
      value4ServoState_1 = 5;
      value4ServoState_2 = 5;
      value4ServoState_3 = 5;
      value4ServoState_6 = 5;
    }
    frame4state4 ++;

    state = 4;
    UP_A_SERVO_rot_target = 0;
    UP_B_SERVO_rot_target = 0;
    UP_C_SERVO_rot_target = 0;

    if (tempindex == 0) {
      DOWN_C_SERVO_rot_target = 0;
    }
  }


  //*************
  // DUR 5
  //*************

  if (distance >= DurA5 && distance < DurB5) {
    if (frame4state5 == 0) {
      tempindex = 0;
      timer = 0;
      value4durChange_5 = 250;


      value4ServoState_1 = 5;
      value4ServoState_2 = 5;
      value4ServoState_3 = 5;
      value4ServoState_4 = 5;
      value4ServoState_5 = 5;
      value4ServoState_6 = 5;
    }
    frame4state5 ++;

    state = 5;

    UP_A_SERVO_rot_target = 0;
    UP_B_SERVO_rot_target = 0;
    UP_C_SERVO_rot_target = 0;
    DOWN_A_SERVO_rot_target = 0;
    DOWN_B_SERVO_rot_target = 0;
    DOWN_C_SERVO_rot_target = 0;
  }
}

绘画函数

// 绘制区间变化时的事件切换[描述]
void updateDuringChangeEffect() {
  push();
  noStroke();
  fill(250, 0, 0, value4durChange_1);
  rect(0+DurA1, height/2-20, DurB1-DurA1, 20);
  pop();
  push();
  noStroke();
  fill(250, 0, 0, value4durChange_2);
  rect(0+DurA2, height/2-20, DurB2-DurA2, 20);
  pop();
  push();
  noStroke();
  fill(250, 0, 0, value4durChange_3);
  rect(0+DurA3, height/2-20, DurB3-DurA3, 20);
  pop();
  push();
  noStroke();
  fill(250, 0, 0, value4durChange_4);
  rect(0+DurA4, height/2-20, DurB4-DurA4, 20);
  pop();
  push();
  noStroke();
  fill(250, 0, 0, value4durChange_5);
  rect(0+DurA5, height/2-20, DurB5-DurA5, 20);
  pop();

  value4durChange_1 -= 12;
  value4durChange_2 -= 12;  
  value4durChange_3 -= 12;  
  value4durChange_4 -= 12;  
  value4durChange_5 -= 12;
  value4durChange_1 = constrain(value4durChange_1, 0, 255);  
  value4durChange_2 = constrain(value4durChange_2, 0, 255);
  value4durChange_3 = constrain(value4durChange_3, 0, 255);
  value4durChange_4 = constrain(value4durChange_4, 0, 255);
  value4durChange_5 = constrain(value4durChange_5, 0, 255);
}

// 绘制每舵机被定义新角度时的触发事件[描述]
void updateServoDefinedEvent()
{

  //1
  if (value4ServoState_1 == 5)
  {
    temp4changekey_1 = true;
  }
  if (temp4changekey_1)
  {
    value4ServoState_1 -= 1;

    if (value4ServoState_1 > 0) {
      value4ServoChange_1 = 230;
    } else {
      value4ServoChange_1 = 0;
      temp4changekey_1 = false;
    }
  }
  // 2
  //////////////////// 
  if (value4ServoState_2 == 5)
  {
    temp4changekey_2 = true;
  }
  if (temp4changekey_2)
  {
    value4ServoState_2 -= 1;

    if (value4ServoState_2 > 0) {
      value4ServoChange_2 = 230;
    } else {
      value4ServoChange_2 = 0;
      temp4changekey_2 = false;
    }
  }

  // 3
  //////////////////// 
  if (value4ServoState_3 == 5)
  {
    temp4changekey_3 = true;
  }
  if (temp4changekey_3)
  {
    value4ServoState_3 -= 1;

    if (value4ServoState_3 > 0) {
      value4ServoChange_3 = 230;
    } else {
      value4ServoChange_3 = 0;
      temp4changekey_3 = false;
    }
  }

  // 4
  //////////////////// 
  if (value4ServoState_4 == 5)
  {
    temp4changekey_4 = true;
  }
  if (temp4changekey_4)
  {
    value4ServoState_4 -= 1;

    if (value4ServoState_4 > 0) {
      value4ServoChange_4 = 230;
    } else {
      value4ServoChange_4 = 0;
      temp4changekey_4 = false;
    }
  }

  // 5
  //////////////////// 
  if (value4ServoState_5 == 5)
  {
    temp4changekey_5 = true;
  }
  if (temp4changekey_5)
  {
    value4ServoState_5 -= 1;

    if (value4ServoState_5 > 0) {
      value4ServoChange_5 = 230;
    } else {
      value4ServoChange_5 = 0;
      temp4changekey_5 = false;
    }
  }
  // 6
  //////////////////// 
  if (value4ServoState_6 == 5)
  {
    temp4changekey_6 = true;
  }
  if (temp4changekey_6)
  {
    value4ServoState_6 -= 1;

    if (value4ServoState_6 > 0) {
      value4ServoChange_6 = 230;
    } else {
      value4ServoChange_6 = 0;
      temp4changekey_6 = false;
    }
  }


  push();
  translate(0, 0);
  fill(250, value4ServoChange_1);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30);
  fill(250, value4ServoChange_2);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*2);
  fill(250, value4ServoChange_3);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*3);
  fill(250, value4ServoChange_4);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*4);
  fill(250, value4ServoChange_5);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*5);
  fill(250, value4ServoChange_6);
  rect(200, 35, 20, 20);
  pop();
}


////////////////////////////////////////////////////////////////////////////
//DRAW FUNCTION
////////////////////////////////////////////////////////////////////////////

void drawUI(){


  updateDuringChangeEffect();
  updateServoDefinedEvent();
  
  //*************
  // 区间
  //*************

  push();
  //noStroke();
  stroke(250);
  strokeWeight(2);
  fill(250, 50);
  rect(0, height/2, DurB1-DurA1, height/2);
  pop();
  push();
  //noStroke();

  stroke(250);
  strokeWeight(2);
  fill(250, 50);
  rect(0+DurA2, height/2, DurB2-DurA2, height/2);
  pop();
  push();
  //noStroke();

  stroke(250);
  strokeWeight(2);
  fill(250, 50);
  rect(0+DurA3, height/2, DurB3-DurA3, height/2);
  pop();
  push();
  //noStroke();

  stroke(250);
  strokeWeight(2);
  fill(250, 50);
  rect(0+DurA4, height/2, DurB4-DurA4, height/2);
  pop();
  push();
  //noStroke();

  stroke(250);
  strokeWeight(2);
  fill(250, 50);
  rect(0+DurA5, height/2, DurB5-DurA5, height/2);
  pop();

  //*************
  // 区间
  //*************

  switch(state)
  {
  case 1:
    push();
    noStroke();
    fill(250, 220);
    rect(0+DurA1, height/2, DurB1-DurA1, height/2);
    pop();
    break;
  case 2:
    push();
    noStroke();
    fill(250, 220);
    rect(0+DurA2, height/2, DurB2-DurA2, height/2);
    pop();
    break;
  case 3:
    push();
    noStroke();
    fill(250, 220);
    rect(0+DurA3, height/2, DurB3-DurA3, height/2);
    pop();
    break;
  case 4:
    push();
    noStroke();
    fill(250, 220);
    rect(0+DurA4, height/2, DurB4-DurA4, height/2);
    pop();
    break;
  case 5:
    push();
    noStroke();
    fill(250, 220);
    rect(0+DurA5, height/2, DurB5-DurA5, height/2);
    pop();
    break;
  }

  push();
  translate(0, 0);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*2);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*3);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*4);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
  push();
  translate(0, 30*5);
  fill(250, 100);
  rect(200, 35, 20, 20);
  pop();
/*
  text("UP_A_SERVO - " + round(UP_A_SERVO_rot), 50, 50);
  text("UP_B_SERVO - " + round(UP_B_SERVO_rot), 50, 50+30*1);
  text("UP_C_SERVO - " + round(UP_C_SERVO_rot), 50, 50+30*2);
  text("DOWN_A_SERVO - " + round(DOWN_A_SERVO_rot), 50, 50+30*3);
  text("DOWN_B_SERVO - " + round(DOWN_B_SERVO_rot), 50, 50+30*4);
  text("DOWN_C_SERVO - " + round(DOWN_C_SERVO_rot), 50, 50+30*5);
*/

// 字符描述 舵机 旋转角度
  text("UP_A_SERVO - " + round(UP_A_SERVO_rot), 50, 50);
  text("UP_B_SERVO - " + round(UP_B_SERVO_rot), 50, 50+30*1);
  text("UP_C_SERVO - " + round(UP_C_SERVO_rot), 50, 50+30*2);
  text("DOWN_A_SERVO - " + round(DOWN_A_SERVO_rot), 50, 50+30*3);
  text("DOWN_B_SERVO - " + round(DOWN_B_SERVO_rot), 50, 50+30*4);
  text("DOWN_C_SERVO - " + round(DOWN_C_SERVO_rot), 50, 50+30*5);

}

主页签

void setup() {
  size(460, 500);
}

void draw() {

  //刷新画布
  background(0);
  //鼠标模拟测距
  if (mouseY >= height/2) {
    distance = mouseX;
  }

  //计算deltatime 每帧流逝时间
  //方便计时
  nowtime = millis();
  deltatime = nowtime - oldtime;
  oldtime = nowtime;

  updateState();  //刷新状态
  handleState();  //执行状态

  // 更新每区间
  updatePerDuring();


  if (valueChangeKey)
  {
    UP_A_SERVO_rot += (UP_A_SERVO_rot_target - UP_A_SERVO_rot) * easing;
    UP_B_SERVO_rot += (UP_B_SERVO_rot_target - UP_B_SERVO_rot) * easing;
    UP_C_SERVO_rot += (UP_C_SERVO_rot_target - UP_C_SERVO_rot) * easing;

    DOWN_A_SERVO_rot += (DOWN_A_SERVO_rot_target - DOWN_A_SERVO_rot) * easing;
    DOWN_B_SERVO_rot += (DOWN_B_SERVO_rot_target - DOWN_B_SERVO_rot) * easing;
    DOWN_C_SERVO_rot += (DOWN_C_SERVO_rot_target - DOWN_C_SERVO_rot) * easing;
  } else
  {
    UP_A_SERVO_rot = getLinearValue(UP_A_SERVO_rot, UP_A_SERVO_rot_target);
    UP_B_SERVO_rot = getLinearValue(UP_B_SERVO_rot, UP_B_SERVO_rot_target);
    UP_C_SERVO_rot = getLinearValue(UP_C_SERVO_rot, UP_C_SERVO_rot_target);
    
    DOWN_A_SERVO_rot = getLinearValue(DOWN_A_SERVO_rot, DOWN_A_SERVO_rot_target);
    DOWN_B_SERVO_rot = getLinearValue(DOWN_B_SERVO_rot, DOWN_B_SERVO_rot_target);
    DOWN_C_SERVO_rot = getLinearValue(DOWN_C_SERVO_rot, DOWN_C_SERVO_rot_target);
  }

  UP_A_SERVO_rot = constrain(UP_A_SERVO_rot, 0, 180);
  UP_B_SERVO_rot = constrain(UP_B_SERVO_rot, 0, 180);
  UP_C_SERVO_rot = constrain(UP_C_SERVO_rot, 0, 180);
  DOWN_A_SERVO_rot = constrain(DOWN_A_SERVO_rot, 0, 180);
  DOWN_B_SERVO_rot = constrain(DOWN_B_SERVO_rot, 0, 180);
  DOWN_C_SERVO_rot = constrain(DOWN_C_SERVO_rot, 0, 180);
  // 绘制 UI
  drawUI();
}

void keyPressed() {
  if (key == ‘1‘) {   //按【1】改变数值过渡方式  线性/缓动
    valueChangeKey = !valueChangeKey;
    println(valueChangeKey);
  
  // 防止 小数值干扰最后四舍五入，且确保数值区间
  UP_A_SERVO_rot = constrain(round(UP_A_SERVO_rot), 0, 180);
  UP_B_SERVO_rot = constrain(round(UP_B_SERVO_rot), 0, 180);
  UP_C_SERVO_rot = constrain(round(UP_C_SERVO_rot), 0, 180);
  DOWN_A_SERVO_rot = constrain(round(DOWN_A_SERVO_rot), 0, 180);
  DOWN_B_SERVO_rot = constrain(round(DOWN_B_SERVO_rot), 0, 180);
  DOWN_C_SERVO_rot = constrain(round(DOWN_C_SERVO_rot), 0, 180);
  }
}

小结

上节源码中有足够的注释，这里就不多赘述。其实思路很清晰，6个舵机每次都随6种状态改变预想的角度值[目标值]，不过有些状态会随计时系统慢慢推进。另外在数值上设立了线性变化和缓动变化两种过渡方式。
如果再增设硬件，比如再多点舵机控制，那么最好是上class 类，也就是把相关函数抽象成类中方法，遍历运行。而且这将是一个系统，包含有事件机制、数值分析与处理、状态机等。不过该篇这样写对于初学者，尤其是在校学生而言，更有参考价值。当然啦，这是纸上谈兵式的模拟演示，要真正接线再上实物那可还得花大量精力和时间，加油共勉_！??

附上最后的模拟效果 ----