the issue i am facing is, when my control enter switch statement, the system hangs, serial com hangs up, I2C bus remains high voltage.
below is the code:
Code: Select all
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_MotorShield.h>
#include <LocateZone.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *Motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *Motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *Motor3 = AFMS.getMotor(3);
#define DEV_Addr 0X3 //Device I2C address in Hex
const ZONE_T virtualZone[ VIRTUAL_ZONE_MAX ] = {
POLYGON_PROCESS, /* process type */
4,
708, 1140, //up button
1228, 1889,
187, 1889,
187, 1889,
/* VZ2 defaults */
POLYGON_PROCESS, /* process type */
4,
708, 907, //down button
188, 158,
1229, 158,
1229, 158,
/* VZ3 defaults */
POLYGON_PROCESS, /* process type */
4,
789, 1023, //left button
1310, 275,
1310, 1772,
1310, 1772,
/* VZ4 defaults */
POLYGON_PROCESS, /* process type */
4,
106, 1771, //right button
305, 1023,
106, 275,
106, 275,
/* VZ5 defaults */
POLYGON_PROCESS, /* process type */
4,
1554, 157, //main up
1926, 157,
1554, 941,
1926, 941,
/* VZ6 defaults */
POLYGON_PROCESS, /* process type */
4,
1555, 1106, //main down
1927, 1106,
1555, 1889,
1927, 1889,
};
unsigned short combinedx = 0; //variables to hold combined values(MSB,LSB) of the X,Y and F
unsigned short combinedy = 0;
unsigned short combinedz = 0;
float X_LOC, Y_LOC,F_FORCE; //Location(mm) and Force(N) values
byte mySensVals[6]; //To store the read I2C buffer data
int Sensitivity = 100, zoneNumber=0;
int sensorValue1 = analogRead(A0);
int sensorValue2 = analogRead(A1);
// Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 3.3V):
float m1pos;
float m2pos;
// print out the value you read:
void setup()
{
Serial.begin(9600);
Wire.begin();
AFMS.begin(64000); // create with the default frequency 64KHz
Motor1->setSpeed(200);
Motor1->run(RELEASE);
Motor2->setSpeed(200);
Motor2->run(RELEASE);
Motor3->setSpeed(200);
Motor3->run(RELEASE);
zoneNumber = 0;
#if 0
TWBR = ((F_CPU / 400000l) - 16) / 2; // Change the i2c clock from 100KHz to 400KHz
#endif
}
void loop()
{
zoneNumber=0;
ReadBuffer();
GetValues();
delay(500);
for (int idx = 0; idx < 6; idx++)
{
if ( zoneNumber == 0 ) /* still no zone found ? Then test the next VBZ */
{
if (IsPointInPolygon( idx ) == true)
{
zoneNumber = idx + 1;
}
}
else
break; /* exit the outer loop. A zone has been found */
}
Serial.print("Zone Number : ");
Serial.println(zoneNumber);
A2DC();
switch (zoneNumber)
{
case 1:
Motor2->run(FORWARD); // turn on motor
delay(1000);
Motor2->run(RELEASE);
break;
case 2:
Motor2->run(BACKWARD); // turn on motor
delay(1000);
Motor2->run(RELEASE);
break;
case 3:
Motor1->run(BACKWARD); // turn on motor
delay(1000);
Motor2->run(RELEASE);
break;
case 4:
Motor1->run(FORWARD); // turn on motor
delay(1000);
Motor2->run(RELEASE);
break;
case 5:
Motor3->run(FORWARD); // turn on motor
delay(1000);
Motor3->run(RELEASE);
break;
case 6:
Motor3->run(BACKWARD); // turn on motor
delay(1000);
Motor3->run(RELEASE);
break;
}
}
// #########################################################################
// determine if point is in the triangle
// #########################################################################
/*******************************************************************************
* This function determines whether measured points are in a Polygon or not
*
* \param VBZidx index to the VBZ being considered
* \retval BOOL True if point lies in polygon
* False if it doesn't
*******************************************************************************/
static bool IsPointInPolygon( unsigned char VBZidx )
{
unsigned char iIdx;
unsigned char jIdx;
unsigned char vertCount;
bool tp_Presence = false;
/* find the number of verticies in this poly less 1*/
vertCount = (unsigned char)virtualZone[VBZidx].polyCount;
/* preset the jIdx to the end of the array of verticies */
jIdx = vertCount - 1U;
for (iIdx = 0; iIdx < vertCount; iIdx++ )
{
if ((( virtualZone[VBZidx].vertex[iIdx].y <= combinedy ) &&
( virtualZone[VBZidx].vertex[jIdx].y >= combinedy )) ||
(( virtualZone[VBZidx].vertex[jIdx].y <= combinedy ) &&
( virtualZone[VBZidx].vertex[iIdx].y >= combinedy )))
{
if (( virtualZone[VBZidx].vertex[iIdx].x + (float)( combinedy - virtualZone[VBZidx].vertex[iIdx].y) / /* PRQA S 3395, 3390, 1800, 4393 */
(float)(virtualZone[VBZidx].vertex[jIdx].y - virtualZone[VBZidx].vertex[iIdx].y) * /*PRQA S 1800, 4393 */
(virtualZone[VBZidx].vertex[jIdx].x - virtualZone[VBZidx].vertex[iIdx].x)) <= combinedx)/* PRQA S 1802 */
{
tp_Presence ^= true; /* an odd count leaves this = TRUE */
}
else { /* MISRA compliance */}
}
else { /* MISRA compliance */}
jIdx = iIdx; /* move down the array of verticies */
}
return (tp_Presence);
}
// #########################################################################
// Read the Data buffer from the POC over I2C
// #########################################################################
void ReadBuffer() //Read the data buffer from the SwitchPack using i2c
{
Wire.beginTransmission(DEV_Addr); // transmit to device
Wire.write(byte(0x10)); // contains data buffer
Wire.write(byte(0x20)); // contains data buffer
Wire.endTransmission(); // stop transmitting
Wire.requestFrom(DEV_Addr, 6); // request 6 bytes from slave device
// receive reading from sensor
while(Wire.available()) // slave may send less than requested
{
int ind;
for (ind =0; ind <= 6; ind++)
{
mySensVals[ind] = Wire.read();
}
}
}
// #########################################################################
// Get the x, y, F values from the read buffer data
// #########################################################################
void GetValues()
{
unsigned char xhi = mySensVals[0]; //Combine the MSB abd LSB data bytes to get the X value
unsigned char xlo = mySensVals[1];
combinedx = xhi;
combinedx = combinedx<<8;
combinedx |=xlo;
Serial.println(combinedx);
unsigned char yhi = mySensVals[2]; //Combine the MSB abd LSB data bytes to get the Y value
unsigned char ylo = mySensVals[3];
combinedy = yhi;
combinedy = combinedy<<8;
combinedy |=ylo;
Serial.println(combinedy);
unsigned char zhi = mySensVals[4]; //Combine the MSB abd LSB data bytes to get the Z value
unsigned char zlo = mySensVals[5];
combinedz = zhi;
combinedz = combinedz<<8;
combinedz |=zlo;
Serial.println(combinedz);
}
//##########################################################################
//Read mirror Position
//##########################################################################
void A2DC()
{
int sensorValue1 = analogRead(A0);
int sensorValue2 = analogRead(A1);
// Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 3.3V):
float voltage1 = sensorValue1 * (3.3 / 1023.0);
float voltage2 = sensorValue2 * (3.3 / 1023.0);
// print out the value you read:
Serial.println(voltage1);
Serial.println(voltage2);
Serial.println();
}
any insights is deeply appreciated
thank you
Prathap