정보나눔

오픈소스하드웨어 프로젝트에 대한 다양한 정보를 나누는 공간입니다.

아두이노 코드 해석
김빠마 | 2017-06-22

아두이노 2 wheel balancing robot 으로 제어기를 짜보려고 하는데요.

C언어가 처음이라 아무것도 모르겠네요...

코드 해석좀 부탁드려도 될까요?

 


/*
 * TWO WHEELER BALANCING
 * 
 * Need to have "I2C dev" folder in Arduino libraries
 * Need to have "MPU6050" folder in Arduino libraries
 * Robot will try and balance itself using the MPU6050 sensor.
 *
 * Best used on OSEPP Mechanical Kits.
 * 2WBAL-01
 *
 * created 08 Nov 2016
 * by Sui Wei
 *
 */

//include motor code
#include "TBMotor.h"

OseppTBMotor Motor1(12, 11);
OseppTBMotor Motor2(8, 3);
//OseppTBMotor Motor3(7, 6);
//OseppTBMotor Motor4(4, 5);

void setup() {
  Serial.begin(115200);
  updateIMU();
  //Change the timer frequency
  //To avoid the frequency of hearing.
  TCCR2B &= ~7;
  TCCR2B |= 1;
}

 

const float offset_a = 2;

int leftSpeed;
int rightSpeed;
float ctrlTurn = 0;
float ctrlwalk = 0;

extern float angle;

float PID_Balance(float e, float kp, float ki, float kd)
{
  static float es = 0, sum = 0;
  float r;
  sum += e;
  if(sum>180)sum=180;else if(sum<-180)sum=-180;
  r = kp * e + ki * sum + kd * (e - es);
  es = e;
  return r;
}
void PIDSetSpeed()
{
  leftSpeed = rightSpeed = PID_Balance(angle+ctrlwalk, 20, 1, 60);
  leftSpeed+=ctrlTurn;
  rightSpeed-=ctrlTurn;

  ctrlTurn *= 0.95;
  ctrlwalk *= 0.9;

  if (leftSpeed > 255)leftSpeed = 255; else if (leftSpeed < -255)leftSpeed = -255;
  if (rightSpeed > 255)rightSpeed = 255; else if (rightSpeed < -255)rightSpeed = -255;
}

void loop() {
  ProcessCommand();
  if(!updateIMU())return;
  
  PIDSetSpeed();

  if (abs(angle) < 45)
  {
    Motor1.SetSpeed(-leftSpeed);
    Motor2.SetSpeed(rightSpeed);
//    Serial.print(a);
//    Serial.print(",");
//    Serial.print(-leftSpeed);
//    Serial.print(",");
//    Serial.print(-rightSpeed);
//    Serial.println(",");
  } else {
    Motor1.SetSpeed(-0);
    Motor2.SetSpeed(-0);
  }
}

void ProcessCommand()
{
  static unsigned long lastCmdIn = 0;
  int c;
  if (Serial.available() > 0 )
  {
    c = Serial.read();
    lastCmdIn = millis();
    switch (c)
    {
      case 'U': case 'u':
        ctrlwalk = 2;
        break;
      case 'd':
        ctrlwalk = - 2;
        break;
      case 'L': case 'l':
        ctrlTurn = -45;
        break;
      case 'R': case 'r':
        ctrlTurn = 45;
        break;
      case 'A': case 'a':
        break;
      case 'C': case 'c':
        break;
      case 'B': case 'b':
        break;
      case 'D':
        break;
      default:
        break;
    }
  } else if (millis() - lastCmdIn > 300) {

  }
}

 

프로필사진

Klant 2017-07-04 08:44:17

처음부터 끝까지의 코드 해석을 말씀하시는건가요?

코드를 해석하시기위해서는 밸런싱 로봇에 사용되는 부품에 대한 사용 방법에 대한 이해부터 숙지되어야 한다고 생각합니다. 

 

IMU 센서를 사용하시더라도 라이브러리 함수에 대한 이해가 없이는 위 코드에 대한 이해는 어렵습니다. 

사용하시게 되는 센서의 목록을 미리 정리해보시는게 좋을 것 같습니다. 

 

자이로 센서에 대한 사용 방법은 아래 링크를 참고하세요~

https://kocoafab.cc/tutorial/view/239

프로필사진

박치원 2019-10-04 20:08:40

void setup() {
  Serial.begin(9600);
  pinMode(12, OUTPUT);
  pinMode(8, OUTPUT);
}

long val=0;
long result=0;
float count=0;

void loop() {
count=count+1;
//val=((count-1)/count)* val +(1/count)  *cell.read();
val= 0.5 * val + 0.5 * cell.read();
result= ((val - 8589461) / 801.82f) ;
Serial.println (result);


if(result>=640) {

for (int thisNote = 0; thisNote < 26; thisNote++) {
int noteDuration = 250 * noteDurations[thisNote];
tone(8, melody[thisNote],noteDuration);
int pauseBetweenNotes = noteDuration * 1.30;
delay(pauseBetweenNotes);
noTone(8);
   digitalWrite(12, HIGH);
   delay(50);
   digitalWrite(12, LOW);
   delay(50);
}
  

}
   else if (result<100)
   digitalWrite(8, LOW);
   delay(1000);

//zero 8589461

}

프로필사진

박치원 2019-10-04 20:09:17

아두이노 코딩 코드 해석좀 부탁드립니다

#include"HX711.h"
#include "pitches.h"

int melody[] = {
NOTE_G4,
NOTE_G4,
NOTE_A5,
NOTE_A5,
NOTE_G4,
NOTE_G4,
NOTE_E4,
NOTE_G4,
NOTE_G4,
NOTE_E4,
NOTE_E4,
NOTE_D4,
0,
NOTE_G4,
NOTE_G4,
NOTE_A5,
NOTE_A5,
NOTE_G4,
NOTE_G4,
NOTE_E4,
NOTE_G4,
NOTE_E4,
NOTE_D4,
NOTE_E4,
NOTE_C4,
0};

int noteDurations[] = {
1,1,1,1,
1,1,2,
1,1,1,1,
3,1,
1,1,1,1,
1,1,2,
1,1,1,1,
3,1};

  HX711 cell(3, 2);

void setup() {
  Serial.begin(9600);
  pinMode(12, OUTPUT);
  pinMode(8, OUTPUT);
}

long val=0;
long result=0;
float count=0;

void loop() {
count=count+1;
//val=((count-1)/count)* val +(1/count)  *cell.read();
val= 0.5 * val + 0.5 * cell.read();
result= ((val - 8589461) / 801.82f) ;
Serial.println (result);


if(result>=640) {

for (int thisNote = 0; thisNote < 26; thisNote++) {
int noteDuration = 250 * noteDurations[thisNote];
tone(8, melody[thisNote],noteDuration);
int pauseBetweenNotes = noteDuration * 1.30;
delay(pauseBetweenNotes);
noTone(8);
   digitalWrite(12, HIGH);
   delay(50);
   digitalWrite(12, LOW);
   delay(50);
}
  

}
   else if (result<100)
   digitalWrite(8, LOW);
   delay(1000);

//zero 8589461

}

프로필사진

손승연 2019-11-19 21:32:36

#include <Servo.h>

Servo servo;
int servo_pin = 2;
float sensor_threshold_high = 2;  // increase this if sensor is too sensitive
float sensor_threshold_low = 1;
float sensor_ambient;

int servo_on = 1200;  // change this value if servo "ON" position is not correct 
int servo_off = 1700; // change this value if servo "OFF" position is not correct
int servo_on_time = 400;  // increase this if servo turns off too quickly

void setup() {
  /* mic sensor power on */
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  digitalWrite(A0, HIGH);
  digitalWrite(A1, LOW);

  /* mic ambient sampling */
  int sample_num = 100;
  long temp = 0;
  for(int i=0; i<sample_num; i++) {
    int sensor_value = analogRead(A2);
    temp += sensor_value;
    delay(1);
  }
  sensor_ambient = temp / sample_num;

  Serial.begin(115200);
  Serial.println(sensor_ambient);
}

bool clapDetected();
void turnSwtich();

void loop() {

  if (clapDetected()) {
    turnSwitch();
  }
}

bool clapDetected() {
  int sensor_value = analogRead(A2);
  if (sensor_value > sensor_ambient + sensor_threshold_high) {
    Serial.print("high");
    for (int i=0; i<5; i++) {
      delay(10);
      sensor_value = analogRead(A2);
      if (sensor_value < sensor_ambient + sensor_threshold_low) {
        Serial.println("clap!");
        return true;
      }
    }
  }
  return false;
}

/* toggle switch on and off */
void turnSwitch() {
  static bool isOn = true;
  servo.attach(servo_pin);
  if (isOn) {
    servo.writeMicroseconds(servo_off);
    isOn = false;
  } else {
    servo.writeMicroseconds(servo_on);
    isOn = true;
  }
  delay(servo_on_time);
  servo.detach();
}

이전글   |    아두이노 메가 2017-06-22
다음글   |    이더넷 쉴드 웹 클라이언트 예제 2017-06-23