/********************************************************************
   Copyright: 2016-2018 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     ROS控制手柄的arduino部分代码,主要就是获取各传感器信息.将其组装好后通过串口
     发送给上位机部分的ROS python代码来解析.
   History:
     20181024:initial this code.
     20181030:修改为发送各模块的原始数据到上位机,数据解析处理在上位机进行,尽可能
         保持该固件代码结构简单,易于调试维护.
*********************************************************************/

#define BAUD_RATE     57600
#define ROTATE_RANGE  100
#define SEND_RATE     10

/*
   define sensors pin
*/
int self_lock_pin = 11;
int handle_x_pin  = A2;
int handle_y_pin  = A1;
int handle_z_pin  = 10;
int rotation_pin  = A0;


void setup() {
  pinMode(self_lock_pin, INPUT);
  pinMode(rotation_pin,  INPUT);
  pinMode(handle_z_pin,  INPUT_PULLUP);

  Serial.begin(BAUD_RATE);
}

void loop() {
  int lock_status    = digitalRead(self_lock_pin);
  int rotation_value = analogRead(rotation_pin);
  int rotate_percent = map(rotation_value, 1023, 0, 0, ROTATE_RANGE);

  //turn left or right
  int handle_x = analogRead(handle_x_pin);

  //move forward or back
  int handle_y = analogRead(handle_y_pin);

  //click handle
  int handle_z = digitalRead(handle_z_pin);

  //output all value by serial port
  outPutData(lock_status, handle_x, handle_y, handle_z, rotate_percent);
  delay(1000 / SEND_RATE);
}

/*
   formate output data
*/
void outPutData(int lock_status, int handle_x, int handle_y, int handle_z, int rotate_percent)
{
  Serial.print(lock_status, DEC);
  Serial.print(" ");
  Serial.print(handle_x, DEC);
  Serial.print(" ");
  Serial.print(handle_y, DEC);
  Serial.print(" ");
  Serial.print(handle_z, DEC);
  Serial.print(" ");
  Serial.println(rotate_percent, DEC);
}