![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
CE713 Drivers and Test Program
Revision 0:4bc9e88c2cff, committed 2017-10-30
- Comitter:
- malcolmlear
- Date:
- Mon Oct 30 11:27:45 2017 +0000
- Commit message:
- CE713 Drivers and Test Program
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Mon Oct 30 11:27:45 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/TextLCD/#308d188a2d3a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 30 11:27:45 2017 +0000 @@ -0,0 +1,278 @@ +// Device Drivers for Labmbed Board + +#include "mbed.h" +#include "TextLCD.h" + +TextLCD lcd(p15, p16, p17, p18, p19, p20); // LCD: RS, E, D4-D7 +SPI spi(p5, p6, p7); // SPI: MOSI, MISO, SCLK (MISO not used with LCD) +DigitalOut lat(p8); // data latch for LED driver TLC59281 +DigitalOut Sel0(p26); // input select bits: +DigitalOut Sel1(p25); // " +DigitalOut Sel2(p24); // " +DigitalIn In0(p14); // input from switches, keypad etc +DigitalIn In1(p13); // " +DigitalIn In2(p12); // " +DigitalIn In3(p11); // " +I2C i2c(p9, p10); // I2C: SDA, SCL pins +PwmOut servo1(p21); // Servo 1 PWM pin +PwmOut servo2(p22); // Servo 2 PWM pin +DigitalOut Trigger(p28); // Ultrasonic rangefinder trigger pin +DigitalIn Echo(p27); // Ultrasonic rangefinder echo pin +Timer Sonar; // Ultrasonic timer + +// global variables +short LEDbits = 0; // global led status used for readback +const int TMP102Addr = 0x92; // TMP102 temperature I2C address +const int MPU6050Addr = 0xd0; // MPU-6050 accelerometer and Gyro I2C address +float Acceleration[3]; // MPU-6050 x,y,z acceleration values in 1G floating point +float GyroRate[3]; // MPU-6050 x,y,z gyrorates in degrees per second +float GyroOffset[3]; // MPU-6050 x,y,z gyrorates compensation +char AReg[] = { 0x3b, 0x3d, 0x3f }; // MPU-6050 I2C x,y,z accelerometer data registers +char GReg[] = { 0x43, 0x45, 0x47 }; // MPU-6050 I2C x,y,z gyro data registers + +void Servo1(float s) { // range +-1 + s=s+1; + if (s>=0 && s<=2) { + servo1.pulsewidth(s/2000+0.001); + } +} + +void Servo2(float s) { // range +-1 + s=s+1; + if (s>=0 && s<=2) { + servo2.pulsewidth(s/2000+0.001); + } +} + +void InitServos() { + servo1.period(0.02); + servo2.period(0.02); + Servo1(0); // initiate servo 1 to centre position + Servo2(0); // initiate servo 1 to centre position +} + +int ReadSonar() { + Trigger = 1; // set sonar trigger pulse high + Sonar.reset(); // reset sonar timer + wait_us(10.0); // 10 us pulse + Trigger = 0; // set sonar trigger pulse low + while (Echo == 0) {}; // wait for echo high (8 cycles have been transmitted) + Sonar.start(); // echo high so start timer + while (Echo == 1) {}; // wait for echo low + Sonar.stop(); // echo low so stop timer + return (Sonar.read_us()*10)/58; // read timer and scale to mm +} + +void InitLEDs() { + lat = 0; // latch must start low + spi.format(16,0); // SPI 16 bit data, low state, high going clock + spi.frequency(1000000); // 1MHz clock rate +} + +void SetLEDs(short ledall) { + LEDbits = ledall; // update global led status + spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1)); + lat = 1; // latch pulse start + lat = 0; // latch pulse end +} + +void SetLED(short LEDNo, short LEDState) { + LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number + LEDState = LEDState & 0x0003; // limit led state + LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' + LEDState = LEDState << LEDNo; + short statemask = ((0x0003 << LEDNo) ^ 0xffff); // mask used to clear led state + LEDbits = ((LEDbits & statemask) | LEDState); // clear and set led state + SetLEDs(LEDbits); +} + +short ReadLED(short LEDNo) { + LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number + LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' + short LEDState = (LEDbits >> LEDNo) & 0x0003; // shift selected led state into ls 2 bits + return LEDState; // return led state +} + +short ReadLEDs() { + return LEDbits; // return led status +} + +void SelInput(short Input) { + Sel0 = Input & 0x0001; // set sel[0:2] pins + Sel1 = (Input >> 1) & 0x0001; // + Sel2 = (Input >> 2) & 0x0001; // +} + +short ReadSwitches() { + SelInput(5); // select least significant 4 switches in[3:0] + short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3); + SelInput(4); // select most significant 4 switches in[3:0] + return (Switches + (In0 << 4) + (In1 << 5) + (In2 << 6) + (In3 << 7)); +} + +short ReadSwitch(short SwitchNo) { + SwitchNo = ((SwitchNo - 1) & 0x0007) + 1; // limit switch number + SwitchNo = 8 - SwitchNo; // offset of switch state in ReadSwitches() + short SwitchState = ReadSwitches(); // read switch states + SwitchState = SwitchState >> SwitchNo; // shift selected switch state into ls bit + return (SwitchState & 0x0001); // mask out and return switch state +} + +short ReadKeys() { + SelInput(0); // select Keypad top row + short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12); + SelInput(1); // select Keypad second row + Keys += (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11); + SelInput(2); // select Keypad third row + Keys += (In0 << 2) + (In1 << 5) + (In2 << 8) + In3; + SelInput(3); // select Keypad forth row + Keys += (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10); + return (Keys ^ 0xffff); // return inverted (Key press active high) +} + +short ReadKey(short KeyNo) { + KeyNo = KeyNo & 0x000f; // limit key number 0 to 15 (0 to F) + short KeyState = ReadKeys(); // read key states + KeyState = KeyState >> KeyNo; // shift selected key state into ls bit + return (KeyState & 0x0001); // mask out and return key state +} + +int FindKeyNo() { + short KeyNo; + short KeyPressed = -1; // set KeyPressed to -1 (no key pressed) + short KeyState = ReadKeys(); // read key states + for (KeyNo= 0; KeyNo < 16; KeyNo++ ) { // check all 16 Keys + if (KeyState & 0x0001) { // check key state + if (KeyPressed == -1) { // check if key already found + KeyPressed = KeyNo; // update KeyPressed + } + else { + return -1; // 2 or more keys pressed + } + } + KeyState = KeyState >> 1; // shift to check next key + } + return KeyPressed; // return KeyPressed +} + +char FindKeyChar() { + short KeyNo; + char KeyChar = ' '; // set KeyChar to ' ' (no key pressed) + KeyNo = FindKeyNo(); // find key pressed + if (KeyNo < 10 && KeyNo >= 0) { + KeyChar = (char) KeyNo + 0x30; // convert char 0-9 to ascii string '0'-'9' + } + if (KeyNo > 9 && KeyNo < 16) { + KeyChar = (char) KeyNo + 0x37; // convert char 10-15 to ascii string 'A'-'F' + } + return KeyChar; // return key pressed +} + +float ReadTemp() { + char Cmd[3]; + Cmd[0] = 0x01; // pointer register value + Cmd[1] = 0x60; // byte 1 of the configuration register + Cmd[2] = 0xa0; // byte 2 of the configuration register + i2c.write(TMP102Addr, Cmd, 3); // select configuration register and write 0x60a0 to it + wait(0.5); // ensure conversion time + Cmd[0] = 0x00; // pointer register value + i2c.write(TMP102Addr, Cmd, 1); // select temperature register + i2c.read(TMP102Addr, Cmd, 2); // read 16-bit temperature register + return (float((Cmd[0] << 8) | Cmd[1]) / 256); // divide by 256 and return temperature +} + +signed short ReadMPU6050(int RegAddr) { + char Cmd[3]; + Cmd[0] = RegAddr; // register address + i2c.write(MPU6050Addr, Cmd, 1); // select register to read + i2c.read(MPU6050Addr, Cmd, 2); // read 2 bytes from register + return ((Cmd[0] << 8) | Cmd[1]); // return signed 16 bit value +} + +void CalibrateGyros() { + short a,b; + for(a=0; a<3; a++) { + GyroOffset[a] = 0; // clear gyro calibration offsets + for(b=0; b<1000; b++) { + GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]); + wait_ms(1); // wait for next sample + } + GyroOffset[a] = GyroOffset[a]/1000; // find average over 1000 samples + } +} + +void InitMotion() { + char Cmd[3]; + Cmd[0] = 0xa1; // config register address + Cmd[1] = 0x06; // accelerometer and gyro bandwidth = 5Hz + i2c.write(MPU6050Addr, Cmd, 2); // write data to config register + Cmd[0] = 0x6b; // power management register address + Cmd[1] = 0x00; // data + i2c.write(MPU6050Addr, Cmd, 2); // write data to power management register + Cmd[0] = 0x1b; // gyro configuration register address + Cmd[1] = 0x08; // no gyro self test, +-500 full scale + i2c.write(MPU6050Addr, Cmd, 2); // write data to gyro configuration register + Cmd[0] = 0x19; // sample rate register address + Cmd[1] = 0x07; // sample rate = gyro output rate / 8 + i2c.write(MPU6050Addr, Cmd, 2); // write data to sample rate register + CalibrateGyros(); +} + +void ReadMotion() { + short a; // Acceleration is in G where 1G = 9.81 ms/s + for(a=0; a<3; a++) { // GyroRate is in degrees per second + Acceleration[a] = (float)ReadMPU6050(AReg[a]) / 16384; + GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5; + } +} + +int main() { + + float spos = 0; // Test servo position + InitLEDs(); + InitMotion(); + InitServos(); + + while(1) { + int a,b; + for (b = 0; b < 4; b++ ) { // select all 4 led states + for (a = 1; a < 9; a++ ) { // set all 8 leds to selected state + SetLED (a,b); // set led 'a' to state 'b' + wait(.05); // wait 0.05 second + } + } + for (a= 1; a < 9; a++ ) { // map Switch states to led's + SetLED (a,(ReadSwitch(a) + 1)); // + wait(.05); // wait 0.05 second + } + float temp = ReadTemp(); // get temperature + lcd.cls(); // clear lcd + lcd.printf("Temp = %f\n", temp); // print temperature + wait(1); // wait 1 second + lcd.cls(); // clear lcd + int swch = ReadSwitches(); // look at Switch states + lcd.printf("Switches = %d\n", swch); // print result + char Key = FindKeyChar(); // look for Key pressed + lcd.printf("Key = %c\n", Key); // print result + wait(1); // wait 1 second + lcd.cls(); // clear lcd + int dist = ReadSonar(); // get distance + lcd.printf("Distance = %d\n", dist); // print result + lcd.printf("Servo = %f\n", spos); // print servo pos + wait(1); // wait 1 second + ReadMotion(); // read new data in from the MPU-6050 + lcd.cls(); // clear lcd + lcd.locate(0,0); // print at start of first line + lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]); + lcd.locate(0,1); // print at start of second line + lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]); + wait(.4); // wait 0.4 second + if (spos < 1) { // is servo at upper limit of 1 + spos += .1; // increment servo position + } // + else { // was at upper limit so + spos = -1; // reset servo position + } // + Servo1(spos); // update servo + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Oct 30 11:27:45 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb \ No newline at end of file