/* Because of the numerous single byte write routines for the clock, a general
* routine provides a simpler interface of doing so */
void write_Ds3231_Byte(byte data, byte reg) {
I2CStartBit();
I2CSend(DS3231_ADDR + I2C_WRITE);
I2CSend(reg);
I2CSend(data);
I2CStopBit();
}
void ImuRead(ImuRegisters & r, bool bDoMag)
{
I2CStart( 0x68,false,2);
I2CSend(0x3B);//59
I2CStop(20);
rv=I2CReadBuf(0x68, (PBYTE)&r, 14);
if(bDoMag)
{
I2CStart(MAGADDR,false,2);
I2CSend(0x03);
I2CStop(20);
rv2=I2CReadBuf(MAGADDR, (PBYTE)&r.mx, 6);
I2CStart(MAGADDR,false,2);
I2CSend(0x0A);
I2CSend(0x01);
I2CStop(20);
}
else
{
r.mx=0;
r.my=0;
r.mz=0;
}
}
void WriteReg(BYTE rg, BYTE val)
{
I2CStart( 0x68,false,2);
I2CSend(rg);
I2CSend(val);
I2CStop(20);
}
// *****************************************************************************
// writes buffer
// *****************************************************************************
void I2Cputs(struct i2c_info *bb, uint8_t *s, uint32_t len)
{
I2CStart(bb);
I2CSend(bb, bb->address * 2); // address
while(len) {
I2CSend(bb, *(s++));
len--;
}
I2CStop(bb); // stop
}
/* Because of the numerous single byte write routines for the clock, a general
* routine provides a simpler interface of doing so */
byte read_Ds3231_Byte(byte reg) {
byte temp;
I2CStartBit(); // start communication
I2CSend(DS3231_ADDR + I2C_WRITE);
I2CSend(reg);
I2CRestartBit();
I2CSend(DS3231_ADDR + I2C_READ);
temp = I2CRead();
I2CNackBit();
I2CStopBit(); // stop transmission
return temp;
}
void init_nunchuck(){
UARTprintf("Initializing Wireless Nunchuck\n\n");
while(1){
I2CSend(0x52<<1, 2, 0xF0, 0x55);
if (I2CMasterErr(I2C0_MASTER_BASE) == I2C_MASTER_ERR_NONE){
Wait(1);
I2CSend(0x52<<1, 2, 0xFB, 0x00);
if (I2CMasterErr(I2C0_MASTER_BASE) == I2C_MASTER_ERR_NONE)
break;
}
Wait(10);
}
}
void I2CFirstThing()
{
WriteReg(0x6B,0x01); //PWR_MGMT_1 X gyro clock
WriteReg(0x37,0x0); //INT_PIN_CFG Turn off I2C slave pass through
WriteReg(0x6A,0x0); //User control
WriteReg(0x1b,0x0); //Gyro config 250 dps
WriteReg(0x1c,0x08); //ACCEL CONFIG 4 G
WriteReg(0x19,0x0); //SAMPLE RATE DIVIDER
WriteReg(0x37, 0x02); //Slave pass throught
OSTimeDly(2);
I2CStart(MAGADDR,false,2);
I2CSend(0x0A);
I2CSend(0x01);
I2CStop(20);
OSTimeDly(2);
}
// *****************************************************************************
// read one byte
// *****************************************************************************
uint8_t I2CGetc(struct i2c_info *bb)
{
uint8_t rv;
I2CStart(bb);
I2CSend(bb, (bb->address * 2)+1); // address
rv = I2CRead(bb, 1);
I2CStop(bb); // stop
return rv;
}
void main()
{
/* Buffer where we will read/write our data */
unsigned char I2CData[] = {0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x09, 0x00};
unsigned char i;
/* Initialize I2C Port */
I2CInit();
/* Send Start condition */
I2CStart();
/* Send DS1307 slave address with write operation */
I2CSend(0xD0);
/* Send subaddress 0x00, we are writing to this location */
I2CSend(0x00);
/* Loop to write 8 bytes */
for (i = 0; i < 8; i++)
{
/* send I2C data one by one */
//I2CSend(I2CInitval[i]);
I2CSend(I2CData[i]);
}
/* Send a stop condition - as transfer finishes */
I2CStop();
/* We will now read data from DS1307 */
/* Reading for a memory based device always starts with a dummy write */
/* Send a start condition */
I2CStart();
/* Send slave address with write */
I2CSend(0xD0);
/* Send address for dummy write operation */
/* this address is actually where we are going to read from */
I2CSend(0x00);
/* Send a repeated start, after a dummy write to start reading */
I2CRestart();
/* send slave address with read bit set */
I2CSend(0xD1);
/* Loop to read 8 bytes from I2C slave */
for (i = 8; i > 0; i--) {
/* read a byte */
I2CData[i] = I2CRead();
/* ACK if its not the last byte to read */
/* if its the last byte then send a NAK */
if (i - 1)
I2CAck();
else
I2CNak();
}
/* Send stop */
I2CStop();
/* end of program */
while(1);
}
int main(void){
char i;
unsigned char data[16];
short wiichuck[7], xinit=0, yinit=0, l_vel, r_vel;
int xpow, ypow;
LockoutProtection();
InitializeMCU();
InitializeUART();
InitializeI2C();
InitializeServos();
SetServoPosition(SERVO_0, 140);
InitializeMotors(true, false);
InitializeEncoders(true, false);
// UARTprintf("Initializing Nunchuck\n\n");
// I2CSend(0x52<<1, 2, 0x40, 0x00);
// Wait(25);
init_nunchuck();
// Wireless Nunchucks Zero @ 128
xinit = yinit = 128;
while(1){
//Start Recalculating Values
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
default:
UARTprintf("WTF: %d\n", I2CMasterErr(I2C0_MASTER_BASE));
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
Wait(1);
I2CRecieve(0x52<<1, data, 6); // Nunchuck data is 6 bytes, but for whatever reason, MEMOREX Wireless Nunchuck wants to send 8...
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
//for(i=0; i<6; i++)
// data[i] = (data[i] ^ 0x17) + 0x17; // Nintendo decided to encrypt thir data...
// Save Joystick Data
wiichuck[0] = data[1]; // X Axis Joystick
wiichuck[1] = data[0]; // Y Axis Joystick
wiichuck[2] = (((unsigned short) data[2]) << 2) + (((unsigned short) data[5]) & (3<<2)); // X Axis Accel
wiichuck[3] = (((unsigned short) data[3]) << 2) + (((unsigned short) data[5]) & (3<<4)); // Y Axis Accel
wiichuck[4] = (((unsigned short) data[4]) << 2) + (((unsigned short) data[5]) & (3<<6)); // Z Axis Accel
wiichuck[5] = data[5] & (1 << 1) ? 0 : 1; //'C' Button
wiichuck[6] = data[5] & (1 << 0) ? 0 : 1; //'Z' Button
//if (xinit == 0 && yinit == 0){
// xinit = wiichuck[0]-127;
// yinit = wiichuck[1]-127;
//}else{
xpow = (wiichuck[0]-xinit)/2;
ypow = (wiichuck[1]-yinit)/2;
l_vel = (xpow - ypow)*2;
r_vel = (xpow + ypow)*2;
l_vel = l_vel > 127 ? 127 : l_vel;
r_vel = r_vel > 127 ? 127 : r_vel;
l_vel = l_vel < -127 ? -127 : l_vel;
r_vel = r_vel < -127 ? -127 : r_vel;
//UARTprintf("X: %d\tY: %d\n", xpow*2, ypow*2);
SetMotorPowers(l_vel / (wiichuck[5]==0 ? 2 : 1), r_vel / (wiichuck[5]==0 ? 2 : 1));
UARTprintf("Motor L: %d\tMotor R: %d\n", l_vel, r_vel);
SetServoPosition(SERVO_0, wiichuck[6]==1 ? 255 : 140);
UARTprintf("Nunchuck Data:\n");
for(i=0; i<7; i++){
UARTprintf(" %d\n", wiichuck[i]);
}NL;
Wait(100);
}
}
}
}
