2019.12.14(pm): Motion Control Car with flex sensor and Xbee module

// 송신부 장갑

#include<SoftwareSerial.h>
#include<Wire.h>

SoftwareSerial xbee(2,3);

/* 
const int MPU = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

int xStable = 96;
int yStable = 90;
*/
int flexpin1 = A5; 
int flexpin2 = A4; 
int flexpin3 = A3; 
int flexpin4 = A2; 

void setup() 
{
    xbee.begin(9600);
    Serial.begin(9600); 
/*
    Wire.begin();
    Wire.beginTransmission(MPU);
    Wire.write(0x6B);  
    Wire.write(0);    
    Wire.endTransmission(true);
    
    acc scale
    Wire.beginTransmission(MPU);
    Wire.write(0x1C);  
    Wire.write(0xF8);     
    Wire.endTransmission(true);
     */
}

void loop() 
{
  /*int xAxis = 0;
  int yAxis = 0;
  int zAxis = 0;
   */
  int flexVal1;
  int flexVal2;
  int flexVal3;
  int flexVal4;
  int sum;
  int data;
  /*
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true);  // request a total of 14 registers
  
  AcX = Wire.read() << 8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

  xAxis = round(-(AcX - 1121) / 32.0);
  yAxis = round((AcY - 942) / 32.0);
  zAxis = AcZ;

  xAxis = constrain(xAxis, -40, 40) + xStable;
  yAxis = constrain(yAxis, -40, 40) + yStable;
   */
  flexVal1 = analogRead(flexpin1);
  flexVal2 = analogRead(flexpin2);
  flexVal3 = analogRead(flexpin3);
  flexVal4 = analogRead(flexpin4);
  
  sum=flexVal1+flexVal2+flexVal3+flexVal4; // 굽혔을 때 1000 이하, 폈을 때 1000이상

  // Serial.print(xAxis);  //왼쪽 90이하 오른쪽 90이상
  // Serial.print("   ");
 //  Serial.print(yAxis);
  // Serial.print("   ");

  Serial.print(flexVal1);
  Serial.print("   ");
  Serial.print(flexVal2);
  Serial.print("   ");
  Serial.print(flexVal3);
  Serial.print("   ");
  Serial.print(flexVal4);
  Serial.print("   ");
  Serial.println(sum);
  
  if(sum<1200) // 정지
  {
    xbee.write(1);
  }
  else if(sum>1600) // 직진
  {
    xbee.write(2);
  }
  else if(sum>1200 && flexVal3<350 && flexVal4<350) // 좌회전
  {
    xbee.write(3);
  }
  else if(sum>1200 && flexVal1<350 && flexVal2<350) // 우회전
  {
    xbee.write(4);
  }
  else if(sum>1200 && flexVal2<350 && flexVal3<350) // 후진
  {
    xbee.write(5);
  }
}

// 수신부 RC카

#include<SoftwareSerial.h>

SoftwareSerial xbee(2,3);

#define STBY 6                 
#define IN1 7                
#define IN2 8                
#define IN3 9              
#define IN4 12               
#define PWM1 11               
#define PWM2 10

void setup() {
  xbee.begin(9600);
  Serial.begin(9600); 
  
  pinMode(STBY, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(PWM1, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(PWM2, OUTPUT);
  digitalWrite(STBY, HIGH);
  analogWrite(PWM1, 128);
  analogWrite(PWM2, 128);
  
}
 
void loop()
{
   if(xbee.read()==1) // 정지
  {
    stopCar();
  }
  else if(xbee.read()==2) // 직진
  {
    forward();
  }
  else if(xbee.read()==3) // 좌회전
  {
    left();
  }
  else if(xbee.read()==4) // 우회전
  {
    right();
  }
  else if(xbee.read()==5) // 후진
  {
    backward();
  }
  else
  {
    stopCar();
  }
delay(50);
 }


// 전진
void forward()
  {
  analogWrite(PWM1, 128);
  analogWrite(PWM2, 128);

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
 
// 후진
void backward() {
  analogWrite(PWM1, 128);
  analogWrite(PWM2, 128);
 
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
 
// 좌회전
void left() {
  analogWrite(PWM1, 128);
  analogWrite(PWM2, 128);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
 
// 우회전
void right() {
  analogWrite(PWM1, 128);
  analogWrite(PWM2, 128);

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

}
 
// 정지
void stopCar() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
    
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}