void main(void)
{
int y = 0;
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
PMAPPWD = 0x02D52; // Get write-access to port mapping regs
P1MAP3 = PM_UCB0SDA; // Map UCB0SDA output to P2.6
P1MAP2 = PM_UCB0SCL; // Map UCB0SCL output to P2.7
PMAPPWD = 0; // Lock port mapping registers
P1SEL |= BIT3 + BIT2; // Select P2.6 & P2.7 to I2C function
HMC5883L_initialize();
while(1)
{
HMC5883L_read_once(magData);
while(y<5000){y++;}
y=0;
}
}
/* Main loop */
int main()
{
/*Clear received array */
memset(&received[0], 0, sizeof(received));
/* Start the SCB UART, Timer and its interrupt */
ModbusUART_Start();
MessageReceived_StartEx(messageReceived_isr);
writeEnable_Write(0); // receive mode
I2C_MASTER_Start(); //Start communicating to HMC5883L
/* Start the encoder interrupt */
SpeedInterrupt_StartEx(speed_isr);
SpeedTimer_Start();
int i = 0;
CyGlobalIntEnable; /* comment this line to disable global interrupts. */
/* Setup Scaling factors for Modbus */
//mb.PIDScaler = 1000;
scaleModbusPIDConstants();
uint8 ida = 0;
HMC5883L_Init();
compassOnline = HMC5883L_testConnection();
if(compassOnline){
HMC5883L_initialize();
}
while(forever)
{
if(modbusMessage)
{
processMessage();
}
if(compassOnline)
{
HMC5883L_getHeading(&cx,&cy,&cz);
sx = (double)(cx) * scale * 10.0;
sy = (double)cy * scale * 10.0;
sz = (double)cz * scale * 10.0;
holdingReg[0] = (int16)sx;
holdingReg[1] = (int16)sy;
if((holdingReg[4] !=0) && (holdingReg[5] !=0))
{
//master has set offsets so it wants us to calculate the bearing here
offsetsx = ((double)((int16)holdingReg[4])) /10.0;
offsetsy = ((double)((int16)holdingReg[5])) /10.0;
bearing = atan2(((sy/10.0) + offsetsy), ((sx/10.0) + offsetsx));
unchangedBearing = bearing;
unchangedBearing = unchangedBearing*(180.0 / M_PI);//convert to degrees
holdingReg[11] = (int16)unchangedBearing; //store to -180 to 180 bearing;
if (bearing < 0)
bearing += 2 * M_PI;
bearing = bearing*(180.0 / M_PI);//convert to degrees
holdingReg[3] = (uint16)bearing*10;
pidSpeed = calculatePID(abs(bearing), abs(holdingReg[10]));
holdingReg[12] = (uint16) pidSpeed;
}
//We don't want to scale the PID contants every time as the floating point
//stuff is wasteful.
if(i < 1000){
i ++;
}
else{
scaleModbusPIDConstants();
i = 0;
}
if(speedInterruptFlag)
{
speedInterruptFlag = 0;
}
}
}
}
