void MainLoop()
{
width = 1;
char data[width]; //register to hold letter sent and received
int i = 0;
while (1)
{
PORTD;
while (!(nrf24l01_irq_pin_active()));
while (!nrf24l01_irq_rx_dr_active());
LED2 = ~LED2;
nrf24l01_read_rx_payload(data, width); //get the payload into data
nrf24l01_irq_clear_all(); //clear interrupts again
for (i = 0; i < width; i++)
{
sprintf(RS232_Out_Buffer, "%c", data[i]);
putsUART1(RS232_Out_Buffer);
}
putsUART1("\r\n");
Delayus(130); //wait for receiver to come from standby to RX
LED3 = ~LED3;
}
}
void gpsSentenceConfig(void){
int i,j;
unsigned char chMsgs [] = "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n\0";
unsigned char chBaudRt [] = "$PMTK251,19200*22\r\n\0";
// Put some huge delays to wait for GPS power-up without the need of a timer
for( i = 0; i < 750; i += 1 ){
for( j = 0; j < 32700; j += 1 )
{
Nop();
}
}
putsUART1((unsigned int *)chMsgs);
while(BusyUART1());
// Put some huge delays to wait for GPS power-up without the need of a timer
for( i = 0; i < 150; i += 1 ){
for( j = 0; j < 32700; j += 1 )
{
Nop();
}
}
putsUART1((unsigned int *)chBaudRt);
while(BusyUART1());
}
//==============================================================================
int i2c_Rx( int Nack )
{
I2C1CONbits.RCEN = 1;
while( !I2C1STATbits.RBF );
if (Nack == 1)
{
I2C1CONbits.ACKDT = 1;
I2C1CONbits.ACKEN = 1;
} else
{
I2C1CONbits.ACKDT = 0;
I2C1CONbits.ACKEN = 1;
}
if(I2CReceiverEnable(EEPROM_I2C_BUS, TRUE) == I2C_RECEIVE_OVERFLOW)
{
putsUART1("Error: I2C Receive Overflow\n");
return FALSE;
}
else
{
int x;
while(!I2CReceivedDataIsAvailable(EEPROM_I2C_BUS));
x = I2CGetByte(EEPROM_I2C_BUS);
while ( I2C1CONbits.ACKEN );
return x;
}
}
int main(void)
{
Initialize(); //initialize IO, UART, SPI, set up nRF24L01 as RX
sprintf(RS232_Out_Buffer, "*** Reciever Intialized (%i)*** \r\n", width);
putsUART1(RS232_Out_Buffer);
MainLoop();
}
//////// INTERRUPTIONS ////////
void __attribute__((interrupt, auto_psv)) _T1Interrupt (void)
{
ledj = ledj^1;
putsUART1("Hello World!\n");
IFS0bits.T1IF = 0 ;
return;
}
void initUART(void) {
__XC_UART = 1;
OpenUART1( UART_EN | UART_NO_PAR_8BIT | UART_1STOPBIT | UART_BRGH_SIXTEEN,
UART_RX_ENABLE | UART_TX_ENABLE,
GetPeripheralClock()/16/BAUD_RATE - 1);
while( BusyUART1()); // Wait until the UART module is free.
putsUART1("Initializing UART1...\n\r");
}
void broadcast_message(const char * message) {
while(BusyUART2()); // Wait until previous transmission is finished
putsUART2 ((unsigned int *)message);
while(BusyUART2()); // Wait until previous transmission is finished
while(BusyUART1()); // Wait until previous transmission is finished
putsUART1 ((unsigned int *)message);
while(BusyUART1()); // Wait until previous transmission is finished
}
BOOL MPU6050::TransmitOneByte( UINT8 data )
{
UINT16 count = 0;
// Wait for the transmitter to be ready
while( !I2CTransmitterIsReady( this->i2cBusId ) && count < 64000 )
{
count++;
}
if( count < 64000 )
{
// Transmit the byte
if(I2CSendByte( this->i2cBusId, data) == I2C_MASTER_BUS_COLLISION)
{
sprintf( filename, "Error in TransmitOneByte(). I2C Master Bus Collision.\n" );
putsUART1( filename );
//DBPRINTF("Error: I2C Master Bus Collision\n");
return FALSE;
}
count = 0;
// Wait for the transmission to finish
while( !I2CTransmissionHasCompleted( this->i2cBusId ) && count < 64000 )
{
count++;
}
if( count >= 64000 )
{
sprintf( filename, "Error: TransmitOneByte(). Loop timeout for I2CTransmissionHasCompleted().\n" );
putsUART1( filename );
return FALSE;
}
}
else
{
sprintf( filename, "Error: TransmitOneByte(). Loop timeout for I2CTransmitterIsReady().\n" );
putsUART1( filename );
return FALSE;
}
return TRUE;
}
void gpsFreqConfig(void){
/*
unsigned char chFreq [] = "$PMTK300,200,0,0,0,0*2F\r\n\0";
putsUART1((unsigned int *)chFreq);
while(BusyUART1());
*/
putsUART1("$PMTK300,999,0,0,0,0*2F\r\n\0");
while(BusyUART1());
}
void __ISR(_UART1_VECTOR, ipl2) IntUart1Handler(void)
{
// Is this an RX interrupt?
if (mU2RXGetIntFlag())
{
// Clear the RX interrupt Flag
mU2RXClearIntFlag();
char change = (char) ReadUART1();
if (change == ']')
{
width++;
if (width > 32)
{
width = width - 32;
}
}
if (change == '[')
{
width--;
if (width < 1)
{
width = width + 32;
}
}
// Echo what we just received.
sprintf(RS232_Out_Buffer, "Width: %d \r\n", width);
putsUART1(RS232_Out_Buffer);
//reset chip to new width
nrf24l01_initialize_debug(true, width, false);
}
// We don't care about TX interrupt
if (mU2TXGetIntFlag())
{
mU2TXClearIntFlag();
}
}
//==============================================================================
BOOL i2c_Tx( UINT8 data )
{
// Wait for the transmitter to be ready
while(!I2CTransmitterIsReady(EEPROM_I2C_BUS));
//while ( !I2C1STATbits.TBF ) ;
// Transmit the byte
if(I2CSendByte(EEPROM_I2C_BUS, data) == I2C_MASTER_BUS_COLLISION)
{
putsUART1("Error: I2C Master Bus Collision\n");
return FALSE;
}
//I2C1TRN = data;
// Wait for the transmission to finish
while(!I2CTransmissionHasCompleted(EEPROM_I2C_BUS));
//while ( I2C1STATbits.TRSTAT );
return TRUE;
}
void MPU6050::StopTransfer( void )
{
I2C_STATUS status;
UINT8 count = 0;
// Send the Stop signal
I2CStop( this->i2cBusId );
// Wait for the signal to complete
do
{
status = I2CGetStatus( this->i2cBusId );
count++;
} while ( !(status & I2C_STOP) && count < 200);
if ( count >= 200 )
{
sprintf( filename, "Error: StopTransfer(). Loop timeout for I2CGetStatus().\n" );
putsUART1( filename );
}
}
void MPU6050::Setup_MPU6050()
{
sprintf(filename, "Starting Setup_MPU6050().\n");
putsUART1( filename );
//mPORTBSetBits(BIT_2);
//Sets sample rate to 8000/1+7 = 1000Hz
writeReg(MPU6050_RA_SMPLRT_DIV, 0x07);
//Disable FSync, 256Hz DLPF
writeReg(MPU6050_RA_CONFIG, 0x00);
//Disable gyro self tests, scale of 500 degrees/s
writeReg(MPU6050_RA_GYRO_CONFIG, 0b00001000);
//Disable accel self tests, scale of +-2g, no DHPF
writeReg(MPU6050_RA_ACCEL_CONFIG, 0x00);
//Freefall threshold of |0mg|
writeReg(MPU6050_RA_FF_THR, 0x00);
//Freefall duration limit of 0
writeReg(MPU6050_RA_FF_DUR, 0x00);
//Motion threshold of 0mg
writeReg(MPU6050_RA_MOT_THR, 0x00);
//Motion duration of 0s
writeReg(MPU6050_RA_MOT_DUR, 0x00);
//Zero motion threshold
writeReg(MPU6050_RA_ZRMOT_THR, 0x00);
//Zero motion duration threshold
writeReg(MPU6050_RA_ZRMOT_DUR, 0x00);
//Disable sensor output to FIFO buffer
writeReg(MPU6050_RA_FIFO_EN, 0x00);
//AUX I2C setup
//Sets AUX I2C to single master control, plus other config
writeReg(MPU6050_RA_I2C_MST_CTRL, 0x00);
//Setup AUX I2C slaves
writeReg(MPU6050_RA_I2C_SLV0_ADDR, 0x00);
writeReg(MPU6050_RA_I2C_SLV0_REG, 0x00);
writeReg(MPU6050_RA_I2C_SLV0_CTRL, 0x00);
writeReg(MPU6050_RA_I2C_SLV1_ADDR, 0x00);
writeReg(MPU6050_RA_I2C_SLV1_REG, 0x00);
writeReg(MPU6050_RA_I2C_SLV1_CTRL, 0x00);
writeReg(MPU6050_RA_I2C_SLV2_ADDR, 0x00);
writeReg(MPU6050_RA_I2C_SLV2_REG, 0x00);
writeReg(MPU6050_RA_I2C_SLV2_CTRL, 0x00);
writeReg(MPU6050_RA_I2C_SLV3_ADDR, 0x00);
writeReg(MPU6050_RA_I2C_SLV3_REG, 0x00);
writeReg(MPU6050_RA_I2C_SLV3_CTRL, 0x00);
writeReg(MPU6050_RA_I2C_SLV4_ADDR, 0x00);
writeReg(MPU6050_RA_I2C_SLV4_REG, 0x00);
writeReg(MPU6050_RA_I2C_SLV4_DO, 0x00);
writeReg(MPU6050_RA_I2C_SLV4_CTRL, 0x00);
writeReg(MPU6050_RA_I2C_SLV4_DI, 0x00);
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
writeReg(MPU6050_RA_INT_PIN_CFG, 0x00);
//Enable data ready interrupt
writeReg(MPU6050_RA_INT_ENABLE, 0x00);
//MPU6050_RA_DMP_INT_STATUS //Read-only
//MPU6050_RA_INT_STATUS 3A //Read-only
//MPU6050_RA_ACCEL_XOUT_H //Read-only
//MPU6050_RA_ACCEL_XOUT_L //Read-only
//MPU6050_RA_ACCEL_YOUT_H //Read-only
//MPU6050_RA_ACCEL_YOUT_L //Read-only
//MPU6050_RA_ACCEL_ZOUT_H //Read-only
//MPU6050_RA_ACCEL_ZOUT_L //Read-only
//MPU6050_RA_TEMP_OUT_H //Read-only
//MPU6050_RA_TEMP_OUT_L //Read-only
//MPU6050_RA_GYRO_XOUT_H //Read-only
//MPU6050_RA_GYRO_XOUT_L //Read-only
//MPU6050_RA_GYRO_YOUT_H //Read-only
//MPU6050_RA_GYRO_YOUT_L //Read-only
//MPU6050_RA_GYRO_ZOUT_H //Read-only
//MPU6050_RA_GYRO_ZOUT_L //Read-only
//MPU6050_RA_EXT_SENS_DATA_00 //Read-only
//MPU6050_RA_EXT_SENS_DATA_01 //Read-only
//MPU6050_RA_EXT_SENS_DATA_02 //Read-only
//MPU6050_RA_EXT_SENS_DATA_03 //Read-only
//MPU6050_RA_EXT_SENS_DATA_04 //Read-only
//MPU6050_RA_EXT_SENS_DATA_05 //Read-only
//MPU6050_RA_EXT_SENS_DATA_06 //Read-only
//MPU6050_RA_EXT_SENS_DATA_07 //Read-only
//MPU6050_RA_EXT_SENS_DATA_08 //Read-only
//MPU6050_RA_EXT_SENS_DATA_09 //Read-only
//MPU6050_RA_EXT_SENS_DATA_10 //Read-only
//MPU6050_RA_EXT_SENS_DATA_11 //Read-only
//MPU6050_RA_EXT_SENS_DATA_12 //Read-only
//MPU6050_RA_EXT_SENS_DATA_13 //Read-only
//MPU6050_RA_EXT_SENS_DATA_14 //Read-only
//MPU6050_RA_EXT_SENS_DATA_15 //Read-only
//MPU6050_RA_EXT_SENS_DATA_16 //Read-only
//MPU6050_RA_EXT_SENS_DATA_17 //Read-only
//MPU6050_RA_EXT_SENS_DATA_18 //Read-only
//MPU6050_RA_EXT_SENS_DATA_19 //Read-only
//MPU6050_RA_EXT_SENS_DATA_20 //Read-only
//MPU6050_RA_EXT_SENS_DATA_21 //Read-only
//MPU6050_RA_EXT_SENS_DATA_22 //Read-only
//MPU6050_RA_EXT_SENS_DATA_23 //Read-only
//MPU6050_RA_MOT_DETECT_STATUS //Read-only
//Slave out, dont care
writeReg(MPU6050_RA_I2C_SLV0_DO, 0x00);
writeReg(MPU6050_RA_I2C_SLV1_DO, 0x00);
writeReg(MPU6050_RA_I2C_SLV2_DO, 0x00);
writeReg(MPU6050_RA_I2C_SLV3_DO, 0x00);
//More slave config
writeReg(MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
//Reset sensor signal paths
writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
//Motion detection control
writeReg(MPU6050_RA_MOT_DETECT_CTRL, 0x00);
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
writeReg(MPU6050_RA_USER_CTRL, 0x00);
//Sets clock source to gyro reference w/ PLL
writeReg(MPU6050_RA_PWR_MGMT_1, 0b00000010);
//Controls frequency of wakeups in accel low power mode plus the sensor standby modes
writeReg(MPU6050_RA_PWR_MGMT_2, 0x00);
//MPU6050_RA_BANK_SEL //Not in datasheet
//MPU6050_RA_MEM_START_ADDR //Not in datasheet
//MPU6050_RA_MEM_R_W //Not in datasheet
//MPU6050_RA_DMP_CFG_1 //Not in datasheet
//MPU6050_RA_DMP_CFG_2 //Not in datasheet
//MPU6050_RA_FIFO_COUNTH //Read-only
//MPU6050_RA_FIFO_COUNTL //Read-only
//Data transfer to and from the FIFO buffer
writeReg(MPU6050_RA_FIFO_R_W, 0x00);
//MPU6050_RA_WHO_AM_I //Read-only, I2C address
//sprintf(filename, "writeReg() completed! STATUS = %d\n", test);
//putsUART1( filename );
}
/* Send command to GSM via UART */
void gsmSendCommand(char *str) {
while(BusyUART1());
putsUART1((unsigned int*)str);
while(BusyUART3());
putsUART3((unsigned int*)str);
}
BOOL MPU6050::writeReg(UINT8 regAddress, UINT8 data)
{
UINT8 i2cData[10];
I2C_7_BIT_ADDRESS SlaveAddress;
int Index;
int DataSz;
BOOL Acknowledged = FALSE;
BOOL Success = TRUE;
//sprintf(filename, "Starting writeReg().\n");
//putsUART1( filename );
// Initialize the data buffer
I2C_FORMAT_7_BIT_ADDRESS(SlaveAddress, this->deviceAddress, I2C_WRITE);
i2cData[0] = SlaveAddress.byte;
i2cData[1] = regAddress; // Register Address to write
i2cData[2] = data; // Data to write
DataSz = 3;
// Start the transfer
if( !StartTransfer(FALSE) )
{
Success = FALSE;
sprintf( filename, "Error in #1 StartTransfer(): when writing address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
// Transmit all data
Index = 0;
while( Success && (Index < DataSz) )
{
// Transmit a byte
if (TransmitOneByte(i2cData[Index++]))
{
// Verify that the byte was acknowledged
if(!I2CByteWasAcknowledged( this->i2cBusId ))
{
Success = FALSE;
sprintf( filename, "Error in #1 I2CByteWasAcknowledged(): when writing address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
else
{
Success = FALSE;
sprintf( filename, "Error in #1 TransmitOneByte(): when writing address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
//sprintf(filename, "Before StopTransfer()\n");
//putsUART1( filename );
// End the transfer
StopTransfer();
//sprintf(filename, "After StopTransfer()\n");
//putsUART1( filename );
// Wait for device to complete write process, by polling the ack status.
while(Acknowledged != TRUE && Success != FALSE)
{
//sprintf(filename, "Inside the loop\n");
//putsUART1( filename );
// Start the transfer
if( StartTransfer(FALSE) )
{
// Transmit just the device's address
if (TransmitOneByte(SlaveAddress.byte))
{
// Check to see if the byte was acknowledged
Acknowledged = I2CByteWasAcknowledged( this->i2cBusId );
/*if( !Acknowledged )
{
sprintf( filename, "!Acknowledged %u.\n", (unsigned)regAddress );
putsUART1( filename );
}*/
}
else
{
Success = FALSE;
sprintf( filename, "Error in #2 TransmitOneByte() - !starttranfer : when writing address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
// End the transfer
StopTransfer();
}
else
{
Success = FALSE;
sprintf( filename, "Error in #2 StartTransfer(): when writing address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
//sprintf(filename, "After the loop\n");
//putsUART1( filename );
if( !Success )
{
sprintf( filename, "Error in writeReg(): when writing to address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
return Success;
}
void process_cmd(void)
{
char goodcmd[] = "- Command Accepted.\n\n\r\0";
char evilcmd[] = "- BAD Command.\n\n\r\0";
char *r = evilcmd;
if (cmdissued == 1)
{
switch(*rbufptr++)
{
/*** STEPPER COMMANDS ***/
case 's':
putcUART1('s');
if(*rbufptr++ == 't'){
putcUART1('t');
switch(*rbufptr++){
case 's':{
putcUART1('s');
putcUART1(' ');
// sts command received (start)
if(*rbufptr++ == ' '){
// sts parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;stepper_enable1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;stepper_enable2=1;
}
}
}break;
case 'p':{
putcUART1('p');
// stp command received (stop)
if(*rbufptr++ == ' '){
// stp parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;stepper_enable1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;stepper_enable2=0;
}
}
}break;
case 'd':{
putcUART1('d');
// std command received (disable)
if(*rbufptr == ' '){
*rbufptr++;
// std parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;disable_stepper1();
}else if(*rbufptr++ == '2'){
r = goodcmd;disable_stepper2();
}
}else{
switch(*rbufptr++){
// stdc command received (direction clockwise)
case 'c':{
if(*rbufptr++ == ' '){
// stdc parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;direction1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;direction2=1;
}
}
}break;
// stdh command received (direction counter clockwise)
case 'h':{
if(*rbufptr++ == ' '){
// stdh parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;direction1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;direction2=0;
}
}
}break;
}
}
}break;
case 'm':{
putcUART1('m');
switch(*rbufptr++){
// stmf command received (Full Step Mode)
case 'f':{
if(*rbufptr++ == ' '){
// stdc parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;mode_step1=1;
}else if(*rbufptr++ == '2'){
r = goodcmd;mode_step2=1;
}
}
}break;
// stmh command received (Half Step Mode)
case 'h':{
if(*rbufptr++ == ' '){
// stdh parameters [1 or 2]
if(*rbufptr == '1'){
r = goodcmd;mode_step1=0;
}else if(*rbufptr++ == '2'){
r = goodcmd;mode_step2=0;
}
}
}break;
}
}break;
case 'f':{
putcUART1('f');
r = goodcmd;
}break;
// invalid command
default: r = evilcmd;break;
}
}
break;
/*** DC MOTOR COMMANDS ***/
case 'm':
putcUART1('m');
if(*rbufptr++ == 'd'){
putcUART1('d');
if(*rbufptr++ == 'c'){
putcUART1('c');
switch(*rbufptr++){
case 'r':{
putcUART1('r');
// mdcr command received (start)
if(*rbufptr++ == ' '){
// mdcr parameters []
r = goodcmd; SetDCOC1PWM(*rbufptr);
}
}break;
case 'p':{
putcUART1('p');
// mdcp command received (start)
if(*rbufptr++ == ' '){
// mdcp parameters []
}
}break;
case 'i':{
putcUART1('i');
// mdci command received (start)
if(*rbufptr++ == ' '){
// mdci parameters []
}
}break;
case 'd':{
putcUART1('d');
// mdcd command received (start)
if(*rbufptr++ == ' '){
// mdcd parameters []
}
}break;
}
}
}
break;
}
rbufptr = (char*)bufstt;
wbufptr = rbufptr;
putsUART1((unsigned int *)r);
while(BusyUART1());
cmdissued = 0;
}
}
// Steps through a lot of random voltages
void RandomLogging(UINT8 node_id, float max_voltage) {
UINT8 i2c_status;
float curr_ang, curry_time = 0.0, swap_time;
UINT16 log_time = 2, log_Ts = 10;
float curr_voltage, sent_voltage;
UINT32 start_millis, start_log_millis, random_store = 0;
i2c_status = disable_motor(node_id);
if (i2c_status != I2C_STATUS_SUCCESFUL) {
sprintf(buf,"Motor with id %d not found on the bus, quitting\n\r",node_id);
putsUART1(buf);
return;
}
sprintf(buf,"\nlog voltages: random, log time: %d sec, Ts: %d ms\n\n\r", log_time, log_Ts);
putsUART1(buf);
putsUART1("| Time | motor angle | motor voltage |\n\n\r");
delay_ms(100);
putsUART1("starting logging:\n\r");
swap_time = ((float) log_time);
calibrate_encoder_zero(node_id);
set_voltage(node_id,0.0);
sent_voltage = 0.0;
start_millis = millis;
while(curry_time < 30.0) {
start_log_millis = millis;
curry_time = ((float)(start_log_millis - start_millis))/1000.0;
i2c_status = get_angle(node_id,&curr_ang);
i2c_status |= get_voltage(node_id,&curr_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
sprintf(buf,"%f, %f, %f\n\r", curry_time, curr_ang, curr_voltage);
putsUART1(buf);
}
// To determine next random voltage
// This should be random enough
//random_store = (random_store << 1) | (ReadTimer45() & 1);
random_store = (random_store << 1) | (read_adc(0) & 1);
if (curry_time > swap_time) {
sent_voltage = max_voltage*((float)random_store)/((float)0xFFFFFFFF);
i2c_status = set_voltage(node_id,sent_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
swap_time = swap_time + ((float) log_time);
RED_LED_SWAP();
}
}
if (GET_BUT2() && curry_time > 2.0) {
start_log_millis = millis;
while( millis - start_log_millis < 1000.0);
break;
}
// Wait until Ts milliseconds has passed since start
while( millis - start_log_millis < log_Ts);
}
putsUART1("logging completed!\n\r");
set_calibration_status_unknown(node_id);
}
void TransientLogging(UINT8 node_id, float log_voltage) {
// Logging parameters
UINT8 i2c_status;
float curr_ang, curr_voltage, curry_time = 0.0, sent_voltage = 0.0, log_time = 1.0;
UINT16 log_Ts = 10;
UINT32 start_millis, start_log_millis;
disable_motor(node_id);
sprintf(buf,"\nlog voltage: %0.1f V, log time: %0.1f sec, Ts: %d ms\n\n\r", log_voltage, log_time, log_Ts);
putsUART1(buf);
putsUART1("| Time | motor angle | motor voltage |\n\n\r");
delay_ms(100);
putsUART1("starting logging:\n\r");
i2c_status = calibrate_encoder_zero(node_id);
if (i2c_status != I2C_STATUS_SUCCESFUL) {
sprintf(buf,"Motor with id %d not found on the bus, quitting\n\r",node_id);
putsUART1(buf);
return;
}
start_millis = millis;
//set_drive_voltage(1,log_voltage);
unsigned char log_stage = 0;
while( curry_time < (2.0 + log_time) ) {
start_log_millis = millis;
curry_time = ((float)(start_log_millis - start_millis))/1000.0;
// Waits one second until activating motor
if ( (log_stage == 0) && (curry_time > 1.0) ){
i2c_status = set_voltage(node_id,log_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 1;
sent_voltage = log_voltage;
}
}
// After log_time set voltage to zero and continue recording
if ( (log_stage == 1) && (curry_time > (log_time + 1.0)) ) {
i2c_status = set_voltage(node_id,0.0); // TODO: LP filter
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 2;
sent_voltage = 0.0;
}
}
i2c_status = get_angle(node_id,&curr_ang);
i2c_status |= get_voltage(node_id,&curr_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
sprintf(buf,"%f, %f, %f\n\r", curry_time, curr_ang, curr_voltage);
putsUART1(buf);
}
// Wait until Ts milliseconds has passed since start
while( millis - start_log_millis < log_Ts);
}
putsUART1("logging completed!\n\r");
set_calibration_status_unknown(node_id);
}
// Control test
// Feedback control test
void ControlLogging(UINT8 node_id) {
// Logging parameters
UINT8 i2c_status;
float curr_ang, curry_time = 0.0, curr_voltage = 0.0, log_time = 2.0;
UINT32 start_millis, start_log_millis, log_Ts = 10;
float reference = 1.5708;
//#define PROGRAM_NEW_PARAMS
#ifdef PROGRAM_NEW_PARAMS
control_params_struct newControlParams =
{
.Fs = 625,
.nd = 2, .d = {-0.6327,0.8222},
.nc = 2, .c = {-0.6327,0.8222},
.nf = 1, .f = {-0.9810}
};
i2c_status = program_control_params(node_id,&newControlParams);
UINT8 prog_status;
if (i2c_status == I2C_STATUS_SUCCESFUL) {
i2c_status = read_control_prog_status(1, &prog_status);
if(i2c_status == I2C_STATUS_SUCCESFUL && prog_status == MOTOR_CONTROL_PROGRAMMING_STATUS_SUCCESS) {
putsUART1("Control programming succesful!\n\r");
} else {
putsUART1("Control programming failed, quitting...\n\r");
return;
}
}
#else
//set_default_control_params(node_id);
#endif
sprintf(buf,"\nreference: %0.2f rad, log time: %0.1f sec, Ts: %d ms\n\n\r", reference, log_time, log_Ts);
putsUART1(buf);
putsUART1("| Time | motor angle | motor voltage |\n\n\r");
delay_ms(100);
putsUART1("starting logging:\n\r");
i2c_status = calibrate_encoder_zero(node_id);
if (i2c_status != I2C_STATUS_SUCCESFUL) {
sprintf(buf,"Motor with id %d not found on the bus, quitting\n\r",node_id);
putsUART1(buf);
return;
}
set_angle(node_id,0.0);
start_millis = millis;
unsigned char log_stage = 0;
while(curry_time < (1.0 + 2.0*log_time)) {
start_log_millis = millis;
curry_time = ((float)(start_log_millis - start_millis))/1000.0;
if ( (log_stage == 0) && (curry_time > 1.0) ){
i2c_status = set_angle(node_id,reference);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 1;
}
}
if ( (log_stage == 1) && curry_time > (1.0 + log_time)) {
i2c_status = set_angle(node_id,0.0);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 2;
}
}
i2c_status = get_angle(node_id,&curr_ang);
i2c_status |= get_voltage(node_id,&curr_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
sprintf(buf,"%f, %f, %f\n\r", curry_time, curr_ang, curr_voltage);
putsUART1(buf);
}
// Wait until Ts milliseconds has passed since start
while( millis - start_log_millis < log_Ts);
}
putsUART1("logging completed!\n\r");
set_calibration_status_unknown(node_id);
}
BOOL MPU6050::StartTransfer( BOOL restart )
{
I2C_STATUS status = I2C_START;
//UINT16 count = 0;
//sprintf(filename, "Starting StartTransfer(), status = %d.\n", status);
//putsUART1( filename );
// Send the Start (or Restart) signal
if(restart)
{
I2C_RESULT res = I2C_SUCCESS;
if((res = I2CRepeatStart( this->i2cBusId )) != I2C_SUCCESS)
{
sprintf(filename, "Repeat start, status = %d.\n",res);
putsUART1( filename );
// Do not return, try to connect anyway and fail
}
}
else
{
// Wait for the bus to be idle, then start the transfer
//while( !I2CBusIsIdle(MPU6050_I2C_BUS) );
// Checks if the bus is idle, and starts the transfer if so
if( I2CBusIsIdle( this->i2cBusId ) )
{
if(I2CStart( this->i2cBusId ) != I2C_SUCCESS)
{
//DBPRINTF("Error: Bus collision during transfer Start\n");
sprintf( filename, "Error in I2CStart(). Bus collision on bus %u during transfer Start.\n", (unsigned)this->i2cBusId );
putsUART1( filename );
return FALSE;
}
}
else
{
sprintf( filename, "Error in I2CBusIsIdle(). Bus %u is not idle.\n", (unsigned)this->i2cBusId );
putsUART1( filename );
return FALSE;
}
}
//sprintf( filename, "StartTransfer(). Checking for Start response...\n" );
//putsUART1( filename );
UINT16 max_tries = 64000, count = 0;
// Wait for the signal to complete or until tries are out
do
{
status = I2CGetStatus( this->i2cBusId );
//sprintf( filename, "StartTransfer(). Status is %u \n", status & I2C_START );
//putsUART1( filename );
} while (!(status & I2C_START) && ++count < max_tries);
if( count >= max_tries )
{
sprintf( filename, "Error in StartTransfer(). Timeout!\n" );
putsUART1( filename );
return FALSE;
}
//sprintf( filename, "StartTransfer(). Function successfully completed!\n" );
//putsUART1( filename );
return TRUE;
}
BOOL MPU6050::readReg( UINT8 regAddress, UINT8 &data )
{
// Variable declarations
UINT8 i2cData[10];
I2C_7_BIT_ADDRESS SlaveAddress;
int Index;
int DataSz;
BOOL Acknowledged;
BOOL Success = TRUE;
UINT8 i2cbyte;
// Initialize the data buffer
I2C_FORMAT_7_BIT_ADDRESS(SlaveAddress, this->deviceAddress, I2C_WRITE);
i2cData[0] = SlaveAddress.byte;
i2cData[1] = regAddress; // MPU6050 data address to read (0x75 = WHO_AM_I which contains 0x68)
DataSz = 2;
// Start the transfer
if( !StartTransfer(FALSE) )
{
//while(1);
Success = FALSE;
sprintf( filename, "Error in #1 StartTransfer(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
// Address the device.
Index = 0;
while( Success & (Index < DataSz) )
{
// Transmit a byte
if (TransmitOneByte(i2cData[Index]))
Index++;
else
{
Success = FALSE;
sprintf( filename, "Error in #1 TransmitOneByte(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
// Verify that the byte was acknowledged
if(!I2CByteWasAcknowledged( this->i2cBusId ))
{
//DBPRINTF("Error: Sent byte was not acknowledged\n");
Success = FALSE;
sprintf( filename, "Error in #1 I2CByteWasAcknowledged(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
// Restart and send the device's internal address to switch to a read transfer
if(Success)
{
// Send a Repeated Started condition
if( !StartTransfer(TRUE) )
{
//while(1);
Success = FALSE;
sprintf( filename, "Error in #2 StartTransfer(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
// Transmit the address with the READ bit set
I2C_FORMAT_7_BIT_ADDRESS(SlaveAddress, this->deviceAddress, I2C_READ);
if (TransmitOneByte(SlaveAddress.byte))
{
// Verify that the byte was acknowledged
if(!I2CByteWasAcknowledged( this->i2cBusId ))
{
//DBPRINTF("Error: Sent byte was not acknowledged\n");
Success = FALSE;
sprintf( filename, "Error in #2 I2CByteWasAcknowledged(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
else
{
Success = FALSE;
sprintf( filename, "Error in #2 TransmitOneByte(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
}
//i2cbyte = 9;
// Read the data from the desired address
if(Success)
{
if(I2CReceiverEnable( this->i2cBusId , TRUE) == I2C_RECEIVE_OVERFLOW)
{
//DBPRINTF("Error: I2C Receive Overflow\n");
Success = FALSE;
sprintf( filename, "Error I2CReceiverEnable(): when reading address %u. I2C Receive Overflow.\n", (unsigned)regAddress );
putsUART1( filename );
}
else
{
while(!I2CReceivedDataIsAvailable( this->i2cBusId ));
i2cbyte = I2CGetByte( this->i2cBusId );
}
}
// End the transfer
StopTransfer();
data = i2cbyte;
if(!Success)
{
//mPORTBSetBits(BIT_2);
//mPORTBClearBits(BIT_3);
sprintf( filename, "Error in readReg(): when reading address %u.\n", (unsigned)regAddress );
putsUART1( filename );
}
return Success;
}