ballCoordinate.cpp

#include "ballCoordinate.h"
#include <stdlib.h>
#include <stdio.h>
#include <arduino.h>

char dataStream[DATALENGTH];
uint8_t numOfPoint;

uint8_t* color;
uint8_t* simpleCoordinate;
int16_t* y;
int16_t* z;

bool getDataStream(void){
  // code Reuse from last year competition

  //first clear all data in the struct
  memset(dataStream, 0, DATALENGTH);

  // Serial Communication with Raspberry Pi on Serial port 1 begins
  Serial.begin(115200);

  Serial.println("GO");

  while(Serial.available() == 0 ){}

  int dataindex = 0;
  while(Serial.available()> 0){

  // Fills datastream
    if(dataindex < DATALENGTH-1)
      {
          char inChar = Serial.read(); // Read a character
          dataStream[dataindex] = inChar; // Store it
          dataindex++; // Increment where to write next
      }
    delayMicroseconds(100);
  }
  dataStream[dataindex] = '\0'; // Null terminate the string
  Serial.flush();

  Serial.print("DataStream(contain): ");Serial.println(dataStream);
  return true;
}

bool parseData(void){

  //split package  to main token by ';'
  char **stringToken = NULL;
  stringToken = split(dataStream,';',&numOfPoint);

  //store number of coordinate is passing
  numOfPoint = atoi(stringToken[0]);

  // exit if receive bad data;
  //TODO: added more validation
  if(numOfPoint > 2 || numOfPoint < 1) {
    //sSerial.println("fail in parse");
    return false;
  }

  //set dynamic array size.
  //create dynamic array size.
  // make sure it only have a size of 1 or 2 acordingly to numOfPoint.
  color = (uint8_t*) malloc(sizeof(uint8_t) * numOfPoint);
  y = (int16_t*) malloc(sizeof(int16_t) * numOfPoint);
  z = (int16_t*) malloc(sizeof(int16_t) * numOfPoint);

  // transformation from pixel coordinate relative to the camera
  // To coordinate that respect to the Robot Arm 
  
  char **pointToken = NULL;
  for(int i = 0;i < numOfPoint; i++){
    uint8_t n = 0 ;// dummies varable for debug or output valiedation
    pointToken = split(stringToken[i+1],',',&n);

    //deposit in to global variable
    color[i] = atoi(pointToken[0]);
    y[i] = atoi(pointToken[1]);
    z[i] = atoi(pointToken[2]);
  }


  // free all 2d array token to prevent mem leak
  for(int i = 0; i < numOfPoint; i++) free(pointToken[i]);
  free(pointToken);
  for(int i = 0; i < numOfPoint; i++) free(stringToken[i]);
  free(stringToken);

  //TODO: ERROR checking and return true if success ful else return false;
  printPoint();
  return true;
}

void printPoint(){
  for(int i = 0; i<numOfPoint;i++){
    Serial.print(color[i]);Serial.print(",");Serial.print(y[i]);Serial.print(",");Serial.print(z[i]);
    Serial.println("");
  }
}

void printSimplePoint(){
  for(int i = 0; i<4;i++){
    Serial.print(color[i]);Serial.println(",");
  }
}

void transformation(double *xVal, double *yVal,const double zVal){
	// convert to real inches value from pixel 
}

bool parseSimpleData(void){
  uint8_t n = 0; //number of coordinate
  //split package  to main token by ';'
  char **stringToken = NULL;
  stringToken = split(dataStream,';',&n);

  //store number of coordinate is passing
  numOfPoint = atoi(stringToken[0]);
  
  // exit if receive bad data;
  //TODO: added more validation
  if(numOfPoint > 2 || numOfPoint < 1) {
    //sSerial.println("fail in parse");
    return false;
  }

  //set dynamic array size.
  //create dynamic array size.
  // make sure it only have a size of 1 or 2 acordingly to numOfPoint.
  color = (uint8_t*) malloc(sizeof(uint8_t) * numOfPoint);
  simpleCoordinate = (int8_t*) malloc(sizeof(int8_t) * numOfPoint);

  char **pointToken = NULL;
  Serial.println("debug");
  Serial.print("n");Serial.println(n);
  for(int i = 0;i < n; i++){
    uint8_t d = 0 ;// dummies varable for debug or output valiedation
    pointToken = split(stringToken[i+1],',',&d);

    //deposit in to global variable

	color[i] = atoi(pointToken[0]);
    Serial.print(color[i]);Serial.println(",");
  }



  // free all 2d array token to prevent mem leak
  for(int i = 0; i < numOfPoint; i++) free(pointToken[i]);
  free(pointToken);
  for(int i = 0; i < numOfPoint; i++) free(stringToken[i]);
  free(stringToken);
	
  return true;
}

char ** split(const char *str, char delimiter, uint8_t *n) {

  int i, numDelimiters = 0;
  char delimit[] = {delimiter, '\0'};
  char *s = deepCopy(str);
  for(i=0; i<strlen(str); i++) {
    if(str[i] == delimiter) {
      numDelimiters++;
    }
  }

  char **result = (char **) malloc(sizeof(char**) * (numDelimiters+1));
  char *token = strtok(s, delimit);
  i = 0;
  while(token != NULL) {
    result[i] = deepCopy(token);
    token = strtok(NULL, delimit);
    i++;
  }

  free(s);
  *n = (numDelimiters+1);
  return result;
}

char * deepCopy(const char *s) {
  char *copy = (char *) malloc( (strlen(s) + 1) * sizeof(char) );
  strcpy(copy, s);
  return copy;
}

ballCoordinate.h

/**
 * @Author Dat nguyen
 *
 * This library establish a protocol for transmitting
 * data from the raspberry pi to arduino
 * The data need to be format in the following manner
 * 2;66,12,32;82,43,53;
 * 1;82,103,12;
 *
 * Where:
 * 2(1st Token): # of coordinate that contain in the package
 * 66(2nd Token,1st Point): represent the color of the ball.represent in ascii of the char 'B'
 * 12(2nd Token,2nd Point): y-axis value being pass
 * 32(2nd Token,3rd Point): z-axis value being pass

 * By using ascii number representation of a char, we will ensure getting a valid Character data.
 * Ascii representation:
 * 'B'=66
 * 'b'=98
 * 'R'=82
 * 'r'=114
 * 'G'=71
 * 'g'=103
 *
 * Unit: Raspbery pi will pass pixel value
 */

 #ifndef BALL_COORDINATES   /* Include guard */
 #define BALL_COORDINATES

#include "stdint.h"
#include "stdlib.h"
#include <Arduino.h>

#define DATALENGTH 100

extern char dataStream[DATALENGTH];
extern uint8_t* color;
extern uint8_t* simpleCoordinate;
extern int16_t * y;
extern int16_t* z;
extern uint8_t numOfPoint;


/**Constant**/
// Raspbery pi info
#define SCREEN_WIDTH 720.00
#define SCREEN_LENGTH 1024.00
#define X_CAMERA_TO_ARM 4.47401575
#define Y_CAMERA_TO_ARM 6.4125

#define COOR_X_1 -5.7
#define COOR_Y_1 2.8
#define COOR_Z_1 7.5

#define COOR_X_2 -5.0
#define COOR_Y_2 2.9
#define COOR_Z_2 10.5

#define COOR_X_3 -5.2
#define COOR_Y_3 5.5
#define COOR_Z_3 12.0

#define COOR_X_4 -5.7
#define COOR_Y_4 7.0
#define COOR_Z_4 9.10

#define COOR_X_DROP -5.0
#define COOR_Y_DROP 9.0
#define COOR_Z_DROP 10.50


// FUNCTION

/**
 * This function print the point exist in 3 global array (color,y,z)
 * return
 * param
 */
void printPoint();

void printSimplePoint(void);

/**
 * This function get data transmited from raspberry pi then store
 * data in dataStream char array  variable(gloabl variable)
 * return 1 if valid dataStream length got recieve
 */
bool getDataStream(void);

/**
 *This function will parse data by using split function
 * to get it token then put it on to a map/struct
 * return true if data is in correct format. or doesnot spot any wierd data.
 * return false; if spot incorrect formatted data.
 */
bool parseData(void);

/**
 * This method compute and transform the pixel coordinate
 * passed over by the PI to actual coordinate in inches
 * that respect to Robot Arm Base
 *
 * param xVal(pixel) and yVal(pixel) and zVal(value retrieve from
 * ultrasonic sensor
 *
 * return Nothing
 * Change xVal and yVal by reference.
 */
void transformation(double *xVal, double *yVal,const double zVal);

/**
 * This method parse the data using split function
 * using hard coded value then send it to the arm.
 * return true if it parse success full
 */
bool parseSimpleData(void);

/**
 * This method convert the given
/**
 * This function split the input string by a delimiter
 * then return a DEEp copy of a 2d array
 * n is the number of element got splited.( optional);
 */
char ** split(const char *str, char delimiter, uint8_t *n);

/*
 * This function deepcopy a array
 */
char * deepCopy(const char *s);

#endif