File content as of revision 5:864709d3eb76:

#ifndef DEBUG_H
#define DEBUG_H

#include "FreeFlyerHardware.h"

void testWheelEncoder(Serial &pc)
{
    pc.printf("Hello World!\n");
    QEI wheel_encoder(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
    Timer t_QEI;
    t_QEI.reset();
    t_QEI.start();
    while (true) {
        if (t_QEI.read_ms() >= 500) {    
            t_QEI.reset();
            pc.printf("Current number of counts: %d\n", wheel_encoder.getPulses());
            pc.printf("Measured wheel speed: %f\n", -wheel_encoder.getSpeed()*COUNTS2SHAFT);    // Shaft direction convention opposite of wheel direction convention
        }
    }
}

void searchForI2C(Serial &pc, I2C &i2c)
{
    pc.printf("I2C Searching!\n\n");
    pc.printf("Starting....\n\n");    
    while (true) {
        int count = 0;
        for (int address=0; address<256; address+=2) {
            if (!i2c.write(address, NULL, 0)) { // 0 returned is ok                
                pc.printf("I2C address 0x%02X\n", address);
                count++;
            }
        }        
        pc.printf("%d devices found\n\n\n", count);
        wait(0.000001);       
    }
}

void testBitMasking(Serial &pc)
{
    char cmd;
    while (true) {
        cmd = 0x00;
        cmd |= (1UL << (0x02-2));
        utils::printBits(cmd, pc);
        pc.printf("\n");
        wait(1.0);
    }
}

void RGBLEDTesting(Serial &pc, I2C &i2c, DigitalOut &led_inv_out_en)
{
    char cmd[19];
    led_inv_out_en = 0;
    
    while (true) {
        
        wait(0.5);
        
        cmd[0] = 0x80;  // Send control register: start with register 00, autoincrement
        cmd[1] = 0x80;  // MODE1 (default value, in normal power mode)
        cmd[2] = 0x09;  // MODE2 (default value, but with invert off, outputs change on ack, open-drain)    
        cmd[3] = 0x00;  // PWM0 (B2)
        cmd[4] = 0x00;  // PWM1 (W2 - ignore)
        cmd[5] = 0x00;  // PWM2 (G2)
        cmd[6] = 0x00;  // PWM3 (R2)
        cmd[7] = 0x00;  // PWM4  (B1)
        cmd[8] = 0x00;  // PWM5 (W1 - ignore)
        cmd[9] = 0x00;  // PWM6 (G1)
        cmd[10] = 0x00;  // PWM7 (R1)
        cmd[11] = 0xff;  // GRPPWM (default value)
        cmd[12] = 0x00;  // GRPFREQ (default value)
        cmd[13] = 0x55;  // LEDOUT0 (0x55: all fully on)
        cmd[14] = 0x55;  // LEDOUT1 (0x55: all fully on)
        cmd[13] = 0x00;  // LEDOUT0 (0x55: all fully off)
        cmd[14] = 0x00;  // LEDOUT1 (0x55: all fully off)
        cmd[15] = 0x00;  // SUBADR1
        cmd[16] = 0x00;  // SUBADR2
        cmd[17] = 0x00;  // SUBADR3
        cmd[18] = 0x00;  // ALLCALLADR
        
        i2c.write(ADDR_RGB, cmd, 19);
        
        //cmd[0] = 0x8d;  // Send control register: start with register 12, autoincrement
        //cmd[1] = 0x55;  // LEDOUT0 (0x55: all fully on)
        //cmd[2] = 0x55;  // LEDOUT1 (0x55: all fully on)
        //cmd[1] = 0x00;  // LEDOUT0 (0x55: all fully off)
        //cmd[2] = 0x00;  // LEDOUT1 (0x55: all fully off)
        
        //mbed::i2c.write(ADDR_RGB, cmd, 3);
        //mbed::i2c.write(mbed::addr, cmd, 3);
        
        cmd[0] = 0x80;
        i2c.write(ADDR_RGB, cmd, 1);
        for (int i = 0; i < 18; i++)
            cmd[i] = 0;
        
        i2c.read(ADDR_RGB, cmd, 18);
        
        for (int i = 0; i < 18; i++) {
            utils::printBits(cmd[i], pc);
            pc.printf("\n");
        }
        pc.printf("\n");
    }
}

#endif