Faiyaz Doctor
/
CE713-V01_Test
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
CE713-V01
by 
malcolm lear
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // Device Drivers for Labmbed Board 00002 00003 #include "mbed.h" 00004 #include "TextLCD.h" 00005 00006 TextLCD lcd(p15, p16, p17, p18, p19, p20); // LCD: RS, E, D4-D7 00007 SPI spi(p5, p6, p7); // SPI: MOSI, MISO, SCLK (MISO not used with LCD) 00008 DigitalOut lat(p8); // data latch for LED driver TLC59281 00009 DigitalOut Sel0(p26); // input select bits: 00010 DigitalOut Sel1(p25); // " 00011 DigitalOut Sel2(p24); // " 00012 DigitalIn In0(p14); // input from switches, keypad etc 00013 DigitalIn In1(p13); // " 00014 DigitalIn In2(p12); // " 00015 DigitalIn In3(p11); // " 00016 I2C i2c(p9, p10); // I2C: SDA, SCL pins 00017 PwmOut servo1(p21); // Servo 1 PWM pin 00018 PwmOut servo2(p22); // Servo 2 PWM pin 00019 DigitalOut Trigger(p28); // Ultrasonic rangefinder trigger pin 00020 DigitalIn Echo(p27); // Ultrasonic rangefinder echo pin 00021 Timer Sonar; // Ultrasonic timer 00022 00023 // global variables 00024 short LEDbits = 0; // global led status used for readback 00025 const int TMP102Addr = 0x92; // TMP102 temperature I2C address 00026 const int MPU6050Addr = 0xd0; // MPU-6050 accelerometer and Gyro I2C address 00027 float Acceleration[3]; // MPU-6050 x,y,z acceleration values in 1G floating point 00028 float GyroRate[3]; // MPU-6050 x,y,z gyrorates in degrees per second 00029 float GyroOffset[3]; // MPU-6050 x,y,z gyrorates compensation 00030 char AReg[] = { 0x3b, 0x3d, 0x3f }; // MPU-6050 I2C x,y,z accelerometer data registers 00031 char GReg[] = { 0x43, 0x45, 0x47 }; // MPU-6050 I2C x,y,z gyro data registers 00032 00033 void Servo1(float s) { // range +-1 00034 s=s+1; 00035 if (s>=0 && s<=2) { 00036 servo1.pulsewidth(s/2000+0.001); 00037 } 00038 } 00039 00040 void Servo2(float s) { // range +-1 00041 s=s+1; 00042 if (s>=0 && s<=2) { 00043 servo2.pulsewidth(s/2000+0.001); 00044 } 00045 } 00046 00047 void InitServos() { 00048 servo1.period(0.02); 00049 servo2.period(0.02); 00050 Servo1(0); // initiate servo 1 to centre position 00051 Servo2(0); // initiate servo 1 to centre position 00052 } 00053 00054 int ReadSonar() { 00055 Trigger = 1; // set sonar trigger pulse high 00056 Sonar.reset(); // reset sonar timer 00057 wait_us(10.0); // 10 us pulse 00058 Trigger = 0; // set sonar trigger pulse low 00059 while (Echo == 0) {}; // wait for echo high (8 cycles have been transmitted) 00060 Sonar.start(); // echo high so start timer 00061 while (Echo == 1) {}; // wait for echo low 00062 Sonar.stop(); // echo low so stop timer 00063 return (Sonar.read_us()*10)/58; // read timer and scale to mm 00064 } 00065 00066 void InitLEDs() { 00067 lat = 0; // latch must start low 00068 spi.format(16,0); // SPI 16 bit data, low state, high going clock 00069 spi.frequency(1000000); // 1MHz clock rate 00070 } 00071 00072 void SetLEDs(short ledall) { 00073 LEDbits = ledall; // update global led status 00074 spi.write((LEDbits & 0x03ff) | ((LEDbits & 0xa800) >> 1) | ((LEDbits & 0x5400) << 1)); 00075 lat = 1; // latch pulse start 00076 lat = 0; // latch pulse end 00077 } 00078 00079 void SetLED(short LEDNo, short LEDState) { 00080 LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number 00081 LEDState = LEDState & 0x0003; // limit led state 00082 LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' 00083 LEDState = LEDState << LEDNo; 00084 short statemask = ((0x0003 << LEDNo) ^ 0xffff); // mask used to clear led state 00085 LEDbits = ((LEDbits & statemask) | LEDState); // clear and set led state 00086 SetLEDs(LEDbits); 00087 } 00088 00089 short ReadLED(short LEDNo) { 00090 LEDNo = ((LEDNo - 1) & 0x0007) + 1; // limit led number 00091 LEDNo = (8 - LEDNo) * 2; // offset of led state in 'LEDbits' 00092 short LEDState = (LEDbits >> LEDNo) & 0x0003; // shift selected led state into ls 2 bits 00093 return LEDState; // return led state 00094 } 00095 00096 short ReadLEDs() { 00097 return LEDbits; // return led status 00098 } 00099 00100 void SelInput(short Input) { 00101 Sel0 = Input & 0x0001; // set sel[0:2] pins 00102 Sel1 = (Input >> 1) & 0x0001; // 00103 Sel2 = (Input >> 2) & 0x0001; // 00104 } 00105 00106 short ReadSwitches() { 00107 SelInput(5); // select least significant 4 switches in[3:0] 00108 short Switches = In0 + (In1 << 1) + (In2 << 2) + (In3 << 3); 00109 SelInput(4); // select most significant 4 switches in[3:0] 00110 return (Switches + (In0 << 4) + (In1 << 5) + (In2 << 6) + (In3 << 7)); 00111 } 00112 00113 short ReadSwitch(short SwitchNo) { 00114 SwitchNo = ((SwitchNo - 1) & 0x0007) + 1; // limit switch number 00115 SwitchNo = 8 - SwitchNo; // offset of switch state in ReadSwitches() 00116 short SwitchState = ReadSwitches(); // read switch states 00117 SwitchState = SwitchState >> SwitchNo; // shift selected switch state into ls bit 00118 return (SwitchState & 0x0001); // mask out and return switch state 00119 } 00120 00121 short ReadKeys() { 00122 SelInput(0); // select Keypad top row 00123 short Keys = (In0 << 15) + (In1 << 14) + (In2 << 13) + (In3 << 12); 00124 SelInput(1); // select Keypad second row 00125 Keys += (In0 << 3) + (In1 << 6) + (In2 << 9) + (In3 << 11); 00126 SelInput(2); // select Keypad third row 00127 Keys += (In0 << 2) + (In1 << 5) + (In2 << 8) + In3; 00128 SelInput(3); // select Keypad forth row 00129 Keys += (In0 << 1) + (In1 << 4) + (In2 << 7) + (In3 << 10); 00130 return (Keys ^ 0xffff); // return inverted (Key press active high) 00131 } 00132 00133 short ReadKey(short KeyNo) { 00134 KeyNo = KeyNo & 0x000f; // limit key number 0 to 15 (0 to F) 00135 short KeyState = ReadKeys(); // read key states 00136 KeyState = KeyState >> KeyNo; // shift selected key state into ls bit 00137 return (KeyState & 0x0001); // mask out and return key state 00138 } 00139 00140 int FindKeyNo() { 00141 short KeyNo; 00142 short KeyPressed = -1; // set KeyPressed to -1 (no key pressed) 00143 short KeyState = ReadKeys(); // read key states 00144 for (KeyNo= 0; KeyNo < 16; KeyNo++ ) { // check all 16 Keys 00145 if (KeyState & 0x0001) { // check key state 00146 if (KeyPressed == -1) { // check if key already found 00147 KeyPressed = KeyNo; // update KeyPressed 00148 } 00149 else { 00150 return -1; // 2 or more keys pressed 00151 } 00152 } 00153 KeyState = KeyState >> 1; // shift to check next key 00154 } 00155 return KeyPressed; // return KeyPressed 00156 } 00157 00158 char FindKeyChar() { 00159 short KeyNo; 00160 char KeyChar = ' '; // set KeyChar to ' ' (no key pressed) 00161 KeyNo = FindKeyNo(); // find key pressed 00162 if (KeyNo < 10 && KeyNo >= 0) { 00163 KeyChar = (char) KeyNo + 0x30; // convert char 0-9 to ascii string '0'-'9' 00164 } 00165 if (KeyNo > 9 && KeyNo < 16) { 00166 KeyChar = (char) KeyNo + 0x37; // convert char 10-15 to ascii string 'A'-'F' 00167 } 00168 return KeyChar; // return key pressed 00169 } 00170 00171 float ReadTemp() { 00172 char Cmd[3]; 00173 Cmd[0] = 0x01; // pointer register value 00174 Cmd[1] = 0x60; // byte 1 of the configuration register 00175 Cmd[2] = 0xa0; // byte 2 of the configuration register 00176 i2c.write(TMP102Addr, Cmd, 3); // select configuration register and write 0x60a0 to it 00177 wait(0.5); // ensure conversion time 00178 Cmd[0] = 0x00; // pointer register value 00179 i2c.write(TMP102Addr, Cmd, 1); // select temperature register 00180 i2c.read(TMP102Addr, Cmd, 2); // read 16-bit temperature register 00181 return (float((Cmd[0] << 8) | Cmd[1]) / 256); // divide by 256 and return temperature 00182 } 00183 00184 signed short ReadMPU6050(int RegAddr) { 00185 char Cmd[3]; 00186 Cmd[0] = RegAddr; // register address 00187 i2c.write(MPU6050Addr, Cmd, 1); // select register to read 00188 i2c.read(MPU6050Addr, Cmd, 2); // read 2 bytes from register 00189 return ((Cmd[0] << 8) | Cmd[1]); // return signed 16 bit value 00190 } 00191 00192 void CalibrateGyros() { 00193 short a,b; 00194 for(a=0; a<3; a++) { 00195 GyroOffset[a] = 0; // clear gyro calibration offsets 00196 for(b=0; b<1000; b++) { 00197 GyroOffset[a] = GyroOffset[a] + (float)ReadMPU6050(GReg[a]); 00198 wait_ms(1); // wait for next sample 00199 } 00200 GyroOffset[a] = GyroOffset[a]/1000; // find average over 1000 samples 00201 } 00202 } 00203 00204 void InitMotion() { 00205 char Cmd[3]; 00206 Cmd[0] = 0xa1; // config register address 00207 Cmd[1] = 0x06; // accelerometer and gyro bandwidth = 5Hz 00208 i2c.write(MPU6050Addr, Cmd, 2); // write data to config register 00209 Cmd[0] = 0x6b; // power management register address 00210 Cmd[1] = 0x00; // data 00211 i2c.write(MPU6050Addr, Cmd, 2); // write data to power management register 00212 Cmd[0] = 0x1b; // gyro configuration register address 00213 Cmd[1] = 0x08; // no gyro self test, +-500 full scale 00214 i2c.write(MPU6050Addr, Cmd, 2); // write data to gyro configuration register 00215 Cmd[0] = 0x19; // sample rate register address 00216 Cmd[1] = 0x07; // sample rate = gyro output rate / 8 00217 i2c.write(MPU6050Addr, Cmd, 2); // write data to sample rate register 00218 CalibrateGyros(); 00219 } 00220 00221 void ReadMotion() { 00222 short a; // Acceleration is in G where 1G = 9.81 ms/s 00223 for(a=0; a<3; a++) { // GyroRate is in degrees per second 00224 Acceleration[a] = (float)ReadMPU6050(AReg[a]) / 16384; 00225 GyroRate[a] = ((float)ReadMPU6050(GReg[a]) - GyroOffset[a]) / 66.5; 00226 } 00227 } 00228 00229 int main() { 00230 00231 float spos = 0; // Test servo position 00232 InitLEDs(); 00233 InitMotion(); 00234 InitServos(); 00235 00236 float temp = -99; 00237 int dist = -99; 00238 char Key = 'X'; 00239 00240 while(1) { 00241 00242 00243 int a,b; 00244 /* 00245 for (b = 0; b < 4; b++ ) { // select all 4 led states 00246 for (a = 1; a < 9; a++ ) { // set all 8 leds to selected state 00247 SetLED (a,b); // set led 'a' to state 'b' 00248 wait(.05); // wait 0.05 second 00249 } 00250 } 00251 */ 00252 00253 int firstOnSwitch = 0; 00254 00255 for (a= 1; a < 9; a++ ) { // map Switch states to led's 00256 SetLED (a,(ReadSwitch(a) + 1)); // 00257 00258 if(ReadSwitch(a) == 1) 00259 { 00260 firstOnSwitch = a; 00261 } 00262 00263 wait(0.05); // wait 0.05 second 00264 } 00265 00266 switch(firstOnSwitch) 00267 { 00268 case 1: 00269 temp = ReadTemp(); // get temperature 00270 lcd.cls(); // clear lcd 00271 lcd.printf("Temp = %f\n", temp); // print temperature 00272 wait(1); // wait 1 second 00273 break; 00274 00275 case 2: 00276 dist = ReadSonar(); // get distance 00277 lcd.cls(); 00278 lcd.printf("Distance = %d\n", dist); // print result 00279 //lcd.printf("Servo = %f\n", spos); // print servo pos 00280 wait(0.5); 00281 break; 00282 00283 case 3: 00284 Key = FindKeyChar(); // look for Key pressed 00285 lcd.cls(); 00286 lcd.printf("Key = %c\n", Key); // print result 00287 wait(0.5); // wait 1 second 00288 break; 00289 00290 // operator is doesn't match any case constant (+, -, *, /) 00291 default: 00292 lcd.cls(); 00293 } 00294 00295 00296 /* 00297 float temp = ReadTemp(); // get temperature 00298 lcd.cls(); // clear lcd 00299 lcd.printf("Temp = %f\n", temp); // print temperature 00300 wait(1); // wait 1 second 00301 lcd.cls(); // clear lcd 00302 00303 00304 int swch = ReadSwitches(); // look at Switch states 00305 lcd.printf("Switches = %d\n", swch); // print result 00306 00307 00308 char Key = FindKeyChar(); // look for Key pressed 00309 lcd.printf("Key = %c\n", Key); // print result 00310 wait(1); // wait 1 second 00311 lcd.cls(); // clear lcd 00312 00313 int dist = ReadSonar(); // get distance 00314 lcd.printf("Distance = %d\n", dist); // print result 00315 lcd.printf("Servo = %f\n", spos); // print servo pos 00316 wait(1); // wait 1 second 00317 00318 ReadMotion(); // read new data in from the MPU-6050 00319 lcd.cls(); // clear lcd 00320 lcd.locate(0,0); // print at start of first line 00321 lcd.printf("x%.1f y%.1f z%.1f", Acceleration[0], Acceleration[1], Acceleration[2]); 00322 lcd.locate(0,1); // print at start of second line 00323 lcd.printf("x%.1f y%.1f z%.1f", GyroRate[0], GyroRate[1], GyroRate[2]); 00324 wait(.4); // wait 0.4 second 00325 00326 if (spos < 1) { // is servo at upper limit of 1 00327 spos += .1; // increment servo position 00328 } // 00329 else { // was at upper limit so 00330 spos = -1; // reset servo position 00331 } // 00332 Servo1(spos); // update servo 00333 */ 00334 } 00335 }
Generated on Tue Aug 2 2022 09:26:06 by
1.7.2