Andrew Schaeffer / Mbed 2 deprecated OU_Davis_Old_Robot

This is the project for the Old Model Robots for OU's Dr. Davis's Configurable Robots Research. This is being published so future robots can be set up easily.

Dependencies:   FatFileSystem MCP3008 Motor PinDetect QTR_8A SRF05 SSD1308_128x64_I2C mbed

main.cpp@0:bcad524c1856, 2017-11-01 (annotated)

Committer:
DrewSchaef
Date:
Wed Nov 01 15:57:59 2017 +0000
Revision:
0:bcad524c1856
Published the project to allow access for future work on the Configurable Robots Research Project(s).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DrewSchaef 0:bcad524c1856 1 //by Austin Saunders
DrewSchaef 0:bcad524c1856 2 //FA2013 - SP2014
DrewSchaef 0:bcad524c1856 3 #include "PowerControl/EthernetPowerControl.h"
DrewSchaef 0:bcad524c1856 4 #include "mbed.h"
DrewSchaef 0:bcad524c1856 5 #include "ReceiverIR.h"
DrewSchaef 0:bcad524c1856 6 #include "TransmitterIR.h"
DrewSchaef 0:bcad524c1856 7 #include "USBHost.h"
DrewSchaef 0:bcad524c1856 8 #include "Utils.h"
DrewSchaef 0:bcad524c1856 9 #include "Motor.h"
DrewSchaef 0:bcad524c1856 10 #include "Wiimote.h"
DrewSchaef 0:bcad524c1856 11 #include "mbed_logo.h"
DrewSchaef 0:bcad524c1856 12 #include "ou_ece_logo.h"
DrewSchaef 0:bcad524c1856 13 #include "countdown.h"
DrewSchaef 0:bcad524c1856 14 #include "battery.h"
DrewSchaef 0:bcad524c1856 15 #include "SSD1308.h"
DrewSchaef 0:bcad524c1856 16 #include "MCP3008.h"
DrewSchaef 0:bcad524c1856 17 #include "QTR_8A.h"
DrewSchaef 0:bcad524c1856 18 #include "PinDetect.h"
DrewSchaef 0:bcad524c1856 19 #include "SRF05.h"
DrewSchaef 0:bcad524c1856 20
DrewSchaef 0:bcad524c1856 21 //Define Pins
DrewSchaef 0:bcad524c1856 22 DigitalOut led1(LED1);
DrewSchaef 0:bcad524c1856 23 DigitalOut led2(LED2);
DrewSchaef 0:bcad524c1856 24 DigitalOut led3(LED3);
DrewSchaef 0:bcad524c1856 25 DigitalOut led4(LED4);
DrewSchaef 0:bcad524c1856 26
DrewSchaef 0:bcad524c1856 27 SRF05 srf(p30, p29);
DrewSchaef 0:bcad524c1856 28
DrewSchaef 0:bcad524c1856 29 ReceiverIR ir_rx_L(p15);//Left TSOP324 38Khz IR Receiver
DrewSchaef 0:bcad524c1856 30 ReceiverIR ir_rx_R(p16);//Right TSOP324 38Khz IR Receiver
DrewSchaef 0:bcad524c1856 31
DrewSchaef 0:bcad524c1856 32 Motor RightMotor(p22,p25,p26);//Right Motor Driven with SN754410 H-Bridge Motor Driver Chip
DrewSchaef 0:bcad524c1856 33 Motor LeftMotor(p21,p23,p24);//Left Motor Driven with SN754410 H-Bridge Motor Driver Chip
DrewSchaef 0:bcad524c1856 34
DrewSchaef 0:bcad524c1856 35 AnalogIn VBatt(p17);//100k-100k voltage divider that measures (Battery Voltage)/2
DrewSchaef 0:bcad524c1856 36
DrewSchaef 0:bcad524c1856 37 AnalogIn CDS_L(p19);//Left Cadmium Sulfide Cell voltage divider to detect light levels
DrewSchaef 0:bcad524c1856 38 AnalogIn CDS_R(p20);//Right Cadmium Sulfide Cell voltage divider to detect light levels
DrewSchaef 0:bcad524c1856 39
DrewSchaef 0:bcad524c1856 40
DrewSchaef 0:bcad524c1856 41 PinDetect sw3(p7);//top momentary switch
DrewSchaef 0:bcad524c1856 42 PinDetect sw2(p6);//bottom momentary switch
DrewSchaef 0:bcad524c1856 43 PinDetect sw1(p5);//middle momentary switch
DrewSchaef 0:bcad524c1856 44
DrewSchaef 0:bcad524c1856 45 //DigitalOut LED_ON(p8);//control signal for QTR sensor LEDs
DrewSchaef 0:bcad524c1856 46
DrewSchaef 0:bcad524c1856 47 I2C i2c(p9, p10);//used for Seeed 128x64 OLED
DrewSchaef 0:bcad524c1856 48 SSD1308 oled = SSD1308(i2c, SSD1308_SA0);//Seeed 128x64 OLED
DrewSchaef 0:bcad524c1856 49
DrewSchaef 0:bcad524c1856 50 QTR_8A qtra(p8);
DrewSchaef 0:bcad524c1856 51
DrewSchaef 0:bcad524c1856 52 // PID terms
DrewSchaef 0:bcad524c1856 53 #define P_TERM 3
DrewSchaef 0:bcad524c1856 54 #define I_TERM 0
DrewSchaef 0:bcad524c1856 55 #define D_TERM 500
DrewSchaef 0:bcad524c1856 56
DrewSchaef 0:bcad524c1856 57 #define MAX .5
DrewSchaef 0:bcad524c1856 58 #define MIN 0
DrewSchaef 0:bcad524c1856 59
DrewSchaef 0:bcad524c1856 60 Ticker ticker;
DrewSchaef 0:bcad524c1856 61 Ticker ticker2;
DrewSchaef 0:bcad524c1856 62
DrewSchaef 0:bcad524c1856 63 unsigned int sensorValues[8];//used for QTR
DrewSchaef 0:bcad524c1856 64
DrewSchaef 0:bcad524c1856 65 volatile int mode;
DrewSchaef 0:bcad524c1856 66 volatile int selection;
DrewSchaef 0:bcad524c1856 67 volatile int count;
DrewSchaef 0:bcad524c1856 68 volatile float speed;
DrewSchaef 0:bcad524c1856 69 Timeout timeout;
DrewSchaef 0:bcad524c1856 70
DrewSchaef 0:bcad524c1856 71 volatile bool calibrated;
DrewSchaef 0:bcad524c1856 72 volatile bool checked;
DrewSchaef 0:bcad524c1856 73 volatile bool pressed;
DrewSchaef 0:bcad524c1856 74
DrewSchaef 0:bcad524c1856 75 volatile bool programmed;
DrewSchaef 0:bcad524c1856 76 volatile bool prog_fwd;
DrewSchaef 0:bcad524c1856 77 volatile bool prog_rev;
DrewSchaef 0:bcad524c1856 78 volatile bool prog_left;
DrewSchaef 0:bcad524c1856 79 volatile bool prog_right;
DrewSchaef 0:bcad524c1856 80 volatile bool prog_up;
DrewSchaef 0:bcad524c1856 81 volatile bool prog_down;
DrewSchaef 0:bcad524c1856 82 volatile bool flipped;
DrewSchaef 0:bcad524c1856 83 volatile bool set;
DrewSchaef 0:bcad524c1856 84
DrewSchaef 0:bcad524c1856 85 volatile float cds_l_max;
DrewSchaef 0:bcad524c1856 86 volatile float cds_r_max;
DrewSchaef 0:bcad524c1856 87 volatile float cds_l_min;
DrewSchaef 0:bcad524c1856 88 volatile float cds_r_min;
DrewSchaef 0:bcad524c1856 89
DrewSchaef 0:bcad524c1856 90 volatile float right;
DrewSchaef 0:bcad524c1856 91 volatile float left;
DrewSchaef 0:bcad524c1856 92 volatile float derivative,proportional,integral;
DrewSchaef 0:bcad524c1856 93 volatile float power;
DrewSchaef 0:bcad524c1856 94 volatile float current_position;
DrewSchaef 0:bcad524c1856 95 volatile float previous_position;
DrewSchaef 0:bcad524c1856 96
DrewSchaef 0:bcad524c1856 97 void set_motors(float l, float r)
DrewSchaef 0:bcad524c1856 98 {
DrewSchaef 0:bcad524c1856 99 LeftMotor.speed(l);
DrewSchaef 0:bcad524c1856 100 RightMotor.speed(r);
DrewSchaef 0:bcad524c1856 101 }
DrewSchaef 0:bcad524c1856 102
DrewSchaef 0:bcad524c1856 103
DrewSchaef 0:bcad524c1856 104 // this is called by the USB infrastructure when a wii message comes in
DrewSchaef 0:bcad524c1856 105 void wii_data(char * data)
DrewSchaef 0:bcad524c1856 106 {
DrewSchaef 0:bcad524c1856 107
DrewSchaef 0:bcad524c1856 108 Wiimote wii;
DrewSchaef 0:bcad524c1856 109 wii.decode(data);
DrewSchaef 0:bcad524c1856 110 if( wii.left ) {
DrewSchaef 0:bcad524c1856 111 set_motors(-speed,speed);
DrewSchaef 0:bcad524c1856 112 } else if( wii.right ) {
DrewSchaef 0:bcad524c1856 113 set_motors(speed,-speed);
DrewSchaef 0:bcad524c1856 114 } else if( wii.up ) {
DrewSchaef 0:bcad524c1856 115 set_motors(speed,speed);
DrewSchaef 0:bcad524c1856 116 } else if( wii.down ) {
DrewSchaef 0:bcad524c1856 117 set_motors(-speed,-speed);
DrewSchaef 0:bcad524c1856 118 } else if (wii.plus) {
DrewSchaef 0:bcad524c1856 119 if(speed < .99) {
DrewSchaef 0:bcad524c1856 120 speed = speed + 0.01;
DrewSchaef 0:bcad524c1856 121 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 122 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 123 }
DrewSchaef 0:bcad524c1856 124 } else if (wii.minus) {
DrewSchaef 0:bcad524c1856 125 if(speed >.26) {
DrewSchaef 0:bcad524c1856 126 speed = speed - 0.01;
DrewSchaef 0:bcad524c1856 127 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 128 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 129 }
DrewSchaef 0:bcad524c1856 130 } else {
DrewSchaef 0:bcad524c1856 131 set_motors(0,0);
DrewSchaef 0:bcad524c1856 132 }
DrewSchaef 0:bcad524c1856 133 float factor = wii.wheel*.015f;
DrewSchaef 0:bcad524c1856 134
DrewSchaef 0:bcad524c1856 135 float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
DrewSchaef 0:bcad524c1856 136 float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
DrewSchaef 0:bcad524c1856 137
DrewSchaef 0:bcad524c1856 138 if( wii.two ) {
DrewSchaef 0:bcad524c1856 139 set_motors(left_factor*speed,right_factor*speed);
DrewSchaef 0:bcad524c1856 140
DrewSchaef 0:bcad524c1856 141 }
DrewSchaef 0:bcad524c1856 142 if( wii.one) {
DrewSchaef 0:bcad524c1856 143 set_motors(-left_factor*speed,-right_factor*speed);
DrewSchaef 0:bcad524c1856 144 }
DrewSchaef 0:bcad524c1856 145
DrewSchaef 0:bcad524c1856 146 }
DrewSchaef 0:bcad524c1856 147
DrewSchaef 0:bcad524c1856 148 float get_batt_volt()//reads in battery voltage from voltage divider (1/2 Vbatt) and multiplies by 2*Vbatt*3.3 (3.3 since ain is fp 0-1)
DrewSchaef 0:bcad524c1856 149 {
DrewSchaef 0:bcad524c1856 150 float batt_volt = VBatt.read();
DrewSchaef 0:bcad524c1856 151 batt_volt = batt_volt*6.6;
DrewSchaef 0:bcad524c1856 152 return batt_volt;
DrewSchaef 0:bcad524c1856 153 }
DrewSchaef 0:bcad524c1856 154
DrewSchaef 0:bcad524c1856 155 float get_batt_perc()
DrewSchaef 0:bcad524c1856 156 {
DrewSchaef 0:bcad524c1856 157 float batt_perc = get_batt_volt();
DrewSchaef 0:bcad524c1856 158 batt_perc = batt_perc-4;
DrewSchaef 0:bcad524c1856 159 batt_perc = batt_perc/2;
DrewSchaef 0:bcad524c1856 160 batt_perc = batt_perc*100;
DrewSchaef 0:bcad524c1856 161 if (batt_perc<0) {
DrewSchaef 0:bcad524c1856 162 batt_perc = 0;
DrewSchaef 0:bcad524c1856 163 } else if (batt_perc>100) {
DrewSchaef 0:bcad524c1856 164 batt_perc = 100;
DrewSchaef 0:bcad524c1856 165 }
DrewSchaef 0:bcad524c1856 166 return batt_perc;
DrewSchaef 0:bcad524c1856 167 }
DrewSchaef 0:bcad524c1856 168
DrewSchaef 0:bcad524c1856 169 //display battery power on OLED
DrewSchaef 0:bcad524c1856 170 void display_batt()
DrewSchaef 0:bcad524c1856 171 {
DrewSchaef 0:bcad524c1856 172 float batt_perc = get_batt_perc();
DrewSchaef 0:bcad524c1856 173 float batt_volt = get_batt_volt();
DrewSchaef 0:bcad524c1856 174 if(batt_perc >= 100) {
DrewSchaef 0:bcad524c1856 175 oled.writeBitmap((uint8_t*) battery_100p);
DrewSchaef 0:bcad524c1856 176 } else if(batt_perc >= 87.5) {
DrewSchaef 0:bcad524c1856 177 oled.writeBitmap((uint8_t*) battery_87_5p);
DrewSchaef 0:bcad524c1856 178 } else if(batt_perc >= 75) {
DrewSchaef 0:bcad524c1856 179 oled.writeBitmap((uint8_t*) battery_75p);
DrewSchaef 0:bcad524c1856 180 } else if(batt_perc >= 62.5) {
DrewSchaef 0:bcad524c1856 181 oled.writeBitmap((uint8_t*) battery_62_5p);
DrewSchaef 0:bcad524c1856 182 } else if(batt_perc >= 50) {
DrewSchaef 0:bcad524c1856 183 oled.writeBitmap((uint8_t*) battery_50p);
DrewSchaef 0:bcad524c1856 184 } else if(batt_perc >= 37.5) {
DrewSchaef 0:bcad524c1856 185 oled.writeBitmap((uint8_t*) battery_37_5p);
DrewSchaef 0:bcad524c1856 186 } else if(batt_perc >= 25) {
DrewSchaef 0:bcad524c1856 187 oled.writeBitmap((uint8_t*) battery_25p);
DrewSchaef 0:bcad524c1856 188 } else if(batt_perc >= 12.5) {
DrewSchaef 0:bcad524c1856 189 oled.writeBitmap((uint8_t*) battery_12_5p);
DrewSchaef 0:bcad524c1856 190 } else {
DrewSchaef 0:bcad524c1856 191 oled.writeBitmap((uint8_t*) battery_0p);
DrewSchaef 0:bcad524c1856 192 }
DrewSchaef 0:bcad524c1856 193 oled.writeString(0,0,"");
DrewSchaef 0:bcad524c1856 194 oled.printf("%.0f%%",batt_perc);
DrewSchaef 0:bcad524c1856 195 oled.writeString(0,12,"");
DrewSchaef 0:bcad524c1856 196 oled.printf("%1.1fV",batt_volt);
DrewSchaef 0:bcad524c1856 197
DrewSchaef 0:bcad524c1856 198 }
DrewSchaef 0:bcad524c1856 199
DrewSchaef 0:bcad524c1856 200
DrewSchaef 0:bcad524c1856 201
DrewSchaef 0:bcad524c1856 202 void set_leds(int l1, int l2, int l3, int l4)
DrewSchaef 0:bcad524c1856 203 {
DrewSchaef 0:bcad524c1856 204 led1 = l1;
DrewSchaef 0:bcad524c1856 205 led2 = l2;
DrewSchaef 0:bcad524c1856 206 led3 = l3;
DrewSchaef 0:bcad524c1856 207 led4 = l4;
DrewSchaef 0:bcad524c1856 208 }
DrewSchaef 0:bcad524c1856 209
DrewSchaef 0:bcad524c1856 210 void led_chase()
DrewSchaef 0:bcad524c1856 211 {
DrewSchaef 0:bcad524c1856 212 set_leds(1,0,0,0);
DrewSchaef 0:bcad524c1856 213 wait(.05);
DrewSchaef 0:bcad524c1856 214 set_leds(0,1,0,0);
DrewSchaef 0:bcad524c1856 215 wait(.05);
DrewSchaef 0:bcad524c1856 216 set_leds(0,0,1,0);
DrewSchaef 0:bcad524c1856 217 wait(.05);
DrewSchaef 0:bcad524c1856 218 set_leds(0,0,0,1);
DrewSchaef 0:bcad524c1856 219 wait(.05);
DrewSchaef 0:bcad524c1856 220 set_leds(0,0,0,0);
DrewSchaef 0:bcad524c1856 221 }
DrewSchaef 0:bcad524c1856 222
DrewSchaef 0:bcad524c1856 223 void display_ou()
DrewSchaef 0:bcad524c1856 224 {
DrewSchaef 0:bcad524c1856 225 oled.writeBitmap((uint8_t*) OU);
DrewSchaef 0:bcad524c1856 226 }
DrewSchaef 0:bcad524c1856 227
DrewSchaef 0:bcad524c1856 228 void display_mbed()
DrewSchaef 0:bcad524c1856 229 {
DrewSchaef 0:bcad524c1856 230 oled.writeBitmap((uint8_t*) mbed_logo);
DrewSchaef 0:bcad524c1856 231 }
DrewSchaef 0:bcad524c1856 232
DrewSchaef 0:bcad524c1856 233 void display_off()
DrewSchaef 0:bcad524c1856 234 {
DrewSchaef 0:bcad524c1856 235 oled.setDisplayOff();
DrewSchaef 0:bcad524c1856 236 }
DrewSchaef 0:bcad524c1856 237
DrewSchaef 0:bcad524c1856 238 void clear_display()
DrewSchaef 0:bcad524c1856 239 {
DrewSchaef 0:bcad524c1856 240 oled.clearDisplay();
DrewSchaef 0:bcad524c1856 241 }
DrewSchaef 0:bcad524c1856 242
DrewSchaef 0:bcad524c1856 243 void display_mode()
DrewSchaef 0:bcad524c1856 244 {
DrewSchaef 0:bcad524c1856 245 if(mode == 0) { //main menu
DrewSchaef 0:bcad524c1856 246 if(selection == 1) {
DrewSchaef 0:bcad524c1856 247 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 248 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 249 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 250 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 251 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 252 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 253 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 254 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 255 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 256 } else if(selection == 2) {
DrewSchaef 0:bcad524c1856 257 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 258 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 259 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 260 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 261 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 262 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 263 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 264 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 265 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 266 } else if(selection == 3) {
DrewSchaef 0:bcad524c1856 267 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 268 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 269 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 270 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 271 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 272 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 273 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 274 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 275 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 276 } else if(selection == 4) {
DrewSchaef 0:bcad524c1856 277 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 278 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 279 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 280 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 281 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 282 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 283 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 284 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 285 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 286 } else if(selection == 5) {
DrewSchaef 0:bcad524c1856 287 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 288 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 289 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 290 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 291 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 292 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 293 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 294 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 295 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 296 } else if(selection == 6) {
DrewSchaef 0:bcad524c1856 297 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 298 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 299 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 300 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 301 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 302 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 303 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 304 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 305 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 306 } else {
DrewSchaef 0:bcad524c1856 307 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 308 oled.writeString(1,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 309 oled.writeString(2,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 310 oled.writeString(3,0,"Line Following");
DrewSchaef 0:bcad524c1856 311 oled.writeString(4,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 312 oled.writeString(5,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 313 oled.setInverted(true);
DrewSchaef 0:bcad524c1856 314 oled.writeString(6,0,"Display Battery");
DrewSchaef 0:bcad524c1856 315 oled.setInverted(false);
DrewSchaef 0:bcad524c1856 316 }
DrewSchaef 0:bcad524c1856 317 } else if(mode == 1) {
DrewSchaef 0:bcad524c1856 318
DrewSchaef 0:bcad524c1856 319 if(!programmed) {
DrewSchaef 0:bcad524c1856 320 oled.writeString(0,0,"Program");
DrewSchaef 0:bcad524c1856 321 oled.writeString(2,0,"Forward:");
DrewSchaef 0:bcad524c1856 322 oled.writeString(3,0,"Reverse:");
DrewSchaef 0:bcad524c1856 323 oled.writeString(4,0,"Left:");
DrewSchaef 0:bcad524c1856 324 oled.writeString(5,0,"Right:");
DrewSchaef 0:bcad524c1856 325 oled.writeString(6,0,"Speed Up:");
DrewSchaef 0:bcad524c1856 326 oled.writeString(7,0,"Speed Dn:");
DrewSchaef 0:bcad524c1856 327 oled.writeString(0,12,"next");
DrewSchaef 0:bcad524c1856 328 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 329 } else {
DrewSchaef 0:bcad524c1856 330 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 331 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 332 oled.writeBigChar(3,55,'3');
DrewSchaef 0:bcad524c1856 333 wait(1);
DrewSchaef 0:bcad524c1856 334 oled.writeBigChar(3,55,'2');
DrewSchaef 0:bcad524c1856 335 wait(1);
DrewSchaef 0:bcad524c1856 336 oled.writeBigChar(3,55,'1');
DrewSchaef 0:bcad524c1856 337 wait(1);
DrewSchaef 0:bcad524c1856 338 oled.writeBitmap((uint8_t*) big_GO);
DrewSchaef 0:bcad524c1856 339 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 340 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 341 led_chase();
DrewSchaef 0:bcad524c1856 342 led_chase();
DrewSchaef 0:bcad524c1856 343 led_chase();
DrewSchaef 0:bcad524c1856 344 clear_display();
DrewSchaef 0:bcad524c1856 345 oled.writeString(0,0,"TV Remote Ctrl");
DrewSchaef 0:bcad524c1856 346 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 347 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 348 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 349 }
DrewSchaef 0:bcad524c1856 350 } else if(mode == 2) {
DrewSchaef 0:bcad524c1856 351 oled.writeString(0,0,"Wiimote Ctrl");
DrewSchaef 0:bcad524c1856 352 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 353
DrewSchaef 0:bcad524c1856 354 } else if(mode == 3) {
DrewSchaef 0:bcad524c1856 355 if(!calibrated) {
DrewSchaef 0:bcad524c1856 356 oled.writeString(0,0,"Calibrate");
DrewSchaef 0:bcad524c1856 357 oled.writeString(1,0,"Sensors");
DrewSchaef 0:bcad524c1856 358 oled.writeString(0,12,"next");
DrewSchaef 0:bcad524c1856 359 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 360 } else {
DrewSchaef 0:bcad524c1856 361 oled.writeString(0,0,"Flash Light Ctrl");
DrewSchaef 0:bcad524c1856 362 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 363 oled.writeBigChar(3,55,'3');
DrewSchaef 0:bcad524c1856 364 wait(1);
DrewSchaef 0:bcad524c1856 365 oled.writeBigChar(3,55,'2');
DrewSchaef 0:bcad524c1856 366 wait(1);
DrewSchaef 0:bcad524c1856 367 oled.writeBigChar(3,55,'1');
DrewSchaef 0:bcad524c1856 368 wait(1);
DrewSchaef 0:bcad524c1856 369 oled.writeBitmap((uint8_t*) big_GO);
DrewSchaef 0:bcad524c1856 370 oled.writeString(0,0,"Flash Light Ctrl!");
DrewSchaef 0:bcad524c1856 371 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 372 led_chase();
DrewSchaef 0:bcad524c1856 373 led_chase();
DrewSchaef 0:bcad524c1856 374 led_chase();
DrewSchaef 0:bcad524c1856 375 clear_display();
DrewSchaef 0:bcad524c1856 376 oled.writeString(0,0,"Flash Light Ctrl!");
DrewSchaef 0:bcad524c1856 377 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 378 }
DrewSchaef 0:bcad524c1856 379 } else if(mode == 4) {
DrewSchaef 0:bcad524c1856 380 if(!calibrated) {
DrewSchaef 0:bcad524c1856 381 oled.writeString(0,0,"Calibrate");
DrewSchaef 0:bcad524c1856 382 oled.writeString(1,0,"Sensors");
DrewSchaef 0:bcad524c1856 383 oled.writeString(0,12,"next");
DrewSchaef 0:bcad524c1856 384 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 385 } else if (!checked) {
DrewSchaef 0:bcad524c1856 386 oled.writeString(0,0,"Check");
DrewSchaef 0:bcad524c1856 387 oled.writeString(1,0,"Calibration");
DrewSchaef 0:bcad524c1856 388 oled.writeString(0,12,"next");
DrewSchaef 0:bcad524c1856 389 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 390 } else {
DrewSchaef 0:bcad524c1856 391 oled.writeString(0,0,"Line Following!");
DrewSchaef 0:bcad524c1856 392 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 393 oled.writeBigChar(3,55,'3');
DrewSchaef 0:bcad524c1856 394 wait(1);
DrewSchaef 0:bcad524c1856 395 oled.writeBigChar(3,55,'2');
DrewSchaef 0:bcad524c1856 396 wait(1);
DrewSchaef 0:bcad524c1856 397 oled.writeBigChar(3,55,'1');
DrewSchaef 0:bcad524c1856 398 wait(1);
DrewSchaef 0:bcad524c1856 399 oled.writeBitmap((uint8_t*) big_GO);
DrewSchaef 0:bcad524c1856 400 oled.writeString(0,0,"Line Following!");
DrewSchaef 0:bcad524c1856 401 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 402 led_chase();
DrewSchaef 0:bcad524c1856 403 led_chase();
DrewSchaef 0:bcad524c1856 404 led_chase();
DrewSchaef 0:bcad524c1856 405 }
DrewSchaef 0:bcad524c1856 406 } else if(mode == 5) {
DrewSchaef 0:bcad524c1856 407 oled.writeString(0,0,"Object Avoidance");
DrewSchaef 0:bcad524c1856 408 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 409 } else if(mode == 6) {
DrewSchaef 0:bcad524c1856 410 oled.writeString(0,0,"Object Seeking");
DrewSchaef 0:bcad524c1856 411 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 412 } else if(mode == 7) {
DrewSchaef 0:bcad524c1856 413 display_batt();
DrewSchaef 0:bcad524c1856 414 oled.writeString(7,12,"back");
DrewSchaef 0:bcad524c1856 415 }
DrewSchaef 0:bcad524c1856 416
DrewSchaef 0:bcad524c1856 417 }
DrewSchaef 0:bcad524c1856 418
DrewSchaef 0:bcad524c1856 419
DrewSchaef 0:bcad524c1856 420
DrewSchaef 0:bcad524c1856 421
DrewSchaef 0:bcad524c1856 422 void sw3_pressed()//top switch
DrewSchaef 0:bcad524c1856 423 {
DrewSchaef 0:bcad524c1856 424 if(mode == 0) {
DrewSchaef 0:bcad524c1856 425 if(selection>1) {
DrewSchaef 0:bcad524c1856 426 selection = selection-1;
DrewSchaef 0:bcad524c1856 427 } else {
DrewSchaef 0:bcad524c1856 428 selection = 1;
DrewSchaef 0:bcad524c1856 429 }
DrewSchaef 0:bcad524c1856 430 display_mode();
DrewSchaef 0:bcad524c1856 431 } else if(mode == 1) {
DrewSchaef 0:bcad524c1856 432 if (!programmed) {
DrewSchaef 0:bcad524c1856 433 programmed = true;
DrewSchaef 0:bcad524c1856 434 }
DrewSchaef 0:bcad524c1856 435 pressed = true;
DrewSchaef 0:bcad524c1856 436 } else if((mode == 4)||(mode == 3)) {
DrewSchaef 0:bcad524c1856 437 if (!calibrated) {
DrewSchaef 0:bcad524c1856 438 calibrated = true;
DrewSchaef 0:bcad524c1856 439 } else if (!checked) {
DrewSchaef 0:bcad524c1856 440 checked = true;
DrewSchaef 0:bcad524c1856 441 } else {
DrewSchaef 0:bcad524c1856 442
DrewSchaef 0:bcad524c1856 443 }
DrewSchaef 0:bcad524c1856 444 //clear_display();
DrewSchaef 0:bcad524c1856 445 //display_mode();
DrewSchaef 0:bcad524c1856 446 pressed = true;
DrewSchaef 0:bcad524c1856 447
DrewSchaef 0:bcad524c1856 448 }
DrewSchaef 0:bcad524c1856 449 }
DrewSchaef 0:bcad524c1856 450
DrewSchaef 0:bcad524c1856 451
DrewSchaef 0:bcad524c1856 452 void sw1_pressed()//middle switch
DrewSchaef 0:bcad524c1856 453 {
DrewSchaef 0:bcad524c1856 454 if(mode==0) {
DrewSchaef 0:bcad524c1856 455
DrewSchaef 0:bcad524c1856 456 mode = selection;
DrewSchaef 0:bcad524c1856 457 selection = 1;
DrewSchaef 0:bcad524c1856 458 clear_display();
DrewSchaef 0:bcad524c1856 459 display_mode();
DrewSchaef 0:bcad524c1856 460 }
DrewSchaef 0:bcad524c1856 461 }
DrewSchaef 0:bcad524c1856 462
DrewSchaef 0:bcad524c1856 463 void sw2_pressed()//bottom switch
DrewSchaef 0:bcad524c1856 464 {
DrewSchaef 0:bcad524c1856 465 if(mode ==0) {
DrewSchaef 0:bcad524c1856 466 if(selection<7) {
DrewSchaef 0:bcad524c1856 467 selection = selection+1;
DrewSchaef 0:bcad524c1856 468 } else {
DrewSchaef 0:bcad524c1856 469 selection = 7;
DrewSchaef 0:bcad524c1856 470 }
DrewSchaef 0:bcad524c1856 471 display_mode();
DrewSchaef 0:bcad524c1856 472 } else if(mode == 1) {
DrewSchaef 0:bcad524c1856 473 if (programmed) {
DrewSchaef 0:bcad524c1856 474 programmed = false;
DrewSchaef 0:bcad524c1856 475 prog_fwd = false;
DrewSchaef 0:bcad524c1856 476 prog_rev = false;
DrewSchaef 0:bcad524c1856 477 prog_left = false;
DrewSchaef 0:bcad524c1856 478 prog_right = false;
DrewSchaef 0:bcad524c1856 479 prog_up = false;
DrewSchaef 0:bcad524c1856 480 prog_down = false;
DrewSchaef 0:bcad524c1856 481 } else {
DrewSchaef 0:bcad524c1856 482 mode = 0;
DrewSchaef 0:bcad524c1856 483 }
DrewSchaef 0:bcad524c1856 484 pressed = true;
DrewSchaef 0:bcad524c1856 485 } else if(mode == 2) {
DrewSchaef 0:bcad524c1856 486 pressed = true;
DrewSchaef 0:bcad524c1856 487 mode = 0;
DrewSchaef 0:bcad524c1856 488 } else if((mode == 4)||(mode ==3)) {
DrewSchaef 0:bcad524c1856 489 if (checked) {
DrewSchaef 0:bcad524c1856 490 checked = false;
DrewSchaef 0:bcad524c1856 491 set_motors(0,0);
DrewSchaef 0:bcad524c1856 492 } else if (calibrated) {
DrewSchaef 0:bcad524c1856 493 calibrated = false;
DrewSchaef 0:bcad524c1856 494 set_motors(0,0);
DrewSchaef 0:bcad524c1856 495 if (mode ==3) {
DrewSchaef 0:bcad524c1856 496 qtra.resetCalibration();
DrewSchaef 0:bcad524c1856 497 } else {
DrewSchaef 0:bcad524c1856 498 cds_r_max = 0;
DrewSchaef 0:bcad524c1856 499 cds_r_min = 1;
DrewSchaef 0:bcad524c1856 500 cds_l_max = 0;
DrewSchaef 0:bcad524c1856 501 cds_l_min = 1;
DrewSchaef 0:bcad524c1856 502 }
DrewSchaef 0:bcad524c1856 503
DrewSchaef 0:bcad524c1856 504 } else {
DrewSchaef 0:bcad524c1856 505 mode = 0;
DrewSchaef 0:bcad524c1856 506 if (mode ==3) {
DrewSchaef 0:bcad524c1856 507 qtra.resetCalibration();
DrewSchaef 0:bcad524c1856 508 } else {
DrewSchaef 0:bcad524c1856 509 cds_r_max = 0;
DrewSchaef 0:bcad524c1856 510 cds_r_min = 1;
DrewSchaef 0:bcad524c1856 511 cds_l_max = 0;
DrewSchaef 0:bcad524c1856 512 cds_l_min = 1;
DrewSchaef 0:bcad524c1856 513 }
DrewSchaef 0:bcad524c1856 514
DrewSchaef 0:bcad524c1856 515
DrewSchaef 0:bcad524c1856 516 }
DrewSchaef 0:bcad524c1856 517 //clear_display();
DrewSchaef 0:bcad524c1856 518 //display_mode();
DrewSchaef 0:bcad524c1856 519 pressed = true;
DrewSchaef 0:bcad524c1856 520
DrewSchaef 0:bcad524c1856 521 } else if( (mode==5)||(mode==6)) {
DrewSchaef 0:bcad524c1856 522 if (set) {
DrewSchaef 0:bcad524c1856 523 set = false;
DrewSchaef 0:bcad524c1856 524 }
DrewSchaef 0:bcad524c1856 525 mode = 0;
DrewSchaef 0:bcad524c1856 526 pressed = true;
DrewSchaef 0:bcad524c1856 527
DrewSchaef 0:bcad524c1856 528 } else {
DrewSchaef 0:bcad524c1856 529
DrewSchaef 0:bcad524c1856 530 mode = 0;
DrewSchaef 0:bcad524c1856 531 clear_display();
DrewSchaef 0:bcad524c1856 532 display_mode();
DrewSchaef 0:bcad524c1856 533 pressed = true;
DrewSchaef 0:bcad524c1856 534
DrewSchaef 0:bcad524c1856 535 }
DrewSchaef 0:bcad524c1856 536 }
DrewSchaef 0:bcad524c1856 537
DrewSchaef 0:bcad524c1856 538 void flash()
DrewSchaef 0:bcad524c1856 539 {
DrewSchaef 0:bcad524c1856 540 led1 = !led1;
DrewSchaef 0:bcad524c1856 541 led2 = !led2;
DrewSchaef 0:bcad524c1856 542 led3 = !led3;
DrewSchaef 0:bcad524c1856 543 led4 = !led4;
DrewSchaef 0:bcad524c1856 544 }
DrewSchaef 0:bcad524c1856 545
DrewSchaef 0:bcad524c1856 546 void flip()
DrewSchaef 0:bcad524c1856 547 {
DrewSchaef 0:bcad524c1856 548 flipped = !flipped;
DrewSchaef 0:bcad524c1856 549 }
DrewSchaef 0:bcad524c1856 550
DrewSchaef 0:bcad524c1856 551 void connect()
DrewSchaef 0:bcad524c1856 552 {
DrewSchaef 0:bcad524c1856 553 oled.writeString(3,0,"Connect Wiimote");
DrewSchaef 0:bcad524c1856 554 }
DrewSchaef 0:bcad524c1856 555
DrewSchaef 0:bcad524c1856 556 void connecting()
DrewSchaef 0:bcad524c1856 557 {
DrewSchaef 0:bcad524c1856 558 oled.writeString(3,0,"Connecting.....");
DrewSchaef 0:bcad524c1856 559 ticker.attach(&flash,.125);
DrewSchaef 0:bcad524c1856 560 }
DrewSchaef 0:bcad524c1856 561
DrewSchaef 0:bcad524c1856 562 void connected()
DrewSchaef 0:bcad524c1856 563 {
DrewSchaef 0:bcad524c1856 564 oled.writeString(3,0,"Connected! ");
DrewSchaef 0:bcad524c1856 565 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 566 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 567 set_leds(0,0,0,1);
DrewSchaef 0:bcad524c1856 568 ticker.detach();
DrewSchaef 0:bcad524c1856 569 }
DrewSchaef 0:bcad524c1856 570
DrewSchaef 0:bcad524c1856 571
DrewSchaef 0:bcad524c1856 572 void Init()
DrewSchaef 0:bcad524c1856 573 {
DrewSchaef 0:bcad524c1856 574 PHY_PowerDown();
DrewSchaef 0:bcad524c1856 575 i2c.frequency(400000);//max freq
DrewSchaef 0:bcad524c1856 576 USBInit();
DrewSchaef 0:bcad524c1856 577 set_leds(0,0,0,0);
DrewSchaef 0:bcad524c1856 578 set_motors(0,0);
DrewSchaef 0:bcad524c1856 579 selection = 1;
DrewSchaef 0:bcad524c1856 580 mode = 0;
DrewSchaef 0:bcad524c1856 581 count = 0;
DrewSchaef 0:bcad524c1856 582 speed = MAX;
DrewSchaef 0:bcad524c1856 583 pressed = 0;
DrewSchaef 0:bcad524c1856 584
DrewSchaef 0:bcad524c1856 585 cds_l_max = 0;
DrewSchaef 0:bcad524c1856 586 cds_r_max = 0;
DrewSchaef 0:bcad524c1856 587 cds_l_min = 1;
DrewSchaef 0:bcad524c1856 588 cds_r_min = 1;
DrewSchaef 0:bcad524c1856 589
DrewSchaef 0:bcad524c1856 590 calibrated = false;
DrewSchaef 0:bcad524c1856 591 checked = false;
DrewSchaef 0:bcad524c1856 592 programmed = false;
DrewSchaef 0:bcad524c1856 593 prog_fwd = false;
DrewSchaef 0:bcad524c1856 594 prog_rev = false;
DrewSchaef 0:bcad524c1856 595 prog_left = false;
DrewSchaef 0:bcad524c1856 596 prog_right = false;
DrewSchaef 0:bcad524c1856 597 prog_up = false;
DrewSchaef 0:bcad524c1856 598 prog_down = false;
DrewSchaef 0:bcad524c1856 599 flipped = false;
DrewSchaef 0:bcad524c1856 600 set = false;
DrewSchaef 0:bcad524c1856 601
DrewSchaef 0:bcad524c1856 602 right = 0;
DrewSchaef 0:bcad524c1856 603 left = 0;
DrewSchaef 0:bcad524c1856 604 derivative = 0;
DrewSchaef 0:bcad524c1856 605 proportional = 0;
DrewSchaef 0:bcad524c1856 606 integral = 0;
DrewSchaef 0:bcad524c1856 607 power = 0;
DrewSchaef 0:bcad524c1856 608 current_position = 0;
DrewSchaef 0:bcad524c1856 609 previous_position = 0;
DrewSchaef 0:bcad524c1856 610
DrewSchaef 0:bcad524c1856 611 sw1.mode( PullUp );
DrewSchaef 0:bcad524c1856 612 sw1.attach_deasserted( &sw1_pressed);
DrewSchaef 0:bcad524c1856 613 sw1.setSampleFrequency();
DrewSchaef 0:bcad524c1856 614
DrewSchaef 0:bcad524c1856 615 sw2.mode( PullUp );
DrewSchaef 0:bcad524c1856 616 sw2.attach_deasserted( &sw2_pressed);
DrewSchaef 0:bcad524c1856 617 sw2.setSampleFrequency();
DrewSchaef 0:bcad524c1856 618
DrewSchaef 0:bcad524c1856 619 sw3.mode( PullUp );
DrewSchaef 0:bcad524c1856 620 sw3.attach_deasserted( &sw3_pressed);
DrewSchaef 0:bcad524c1856 621 sw3.setSampleFrequency();
DrewSchaef 0:bcad524c1856 622 oled.setContrastControl(0);
DrewSchaef 0:bcad524c1856 623 display_mbed();
DrewSchaef 0:bcad524c1856 624 led_chase();
DrewSchaef 0:bcad524c1856 625 led_chase();
DrewSchaef 0:bcad524c1856 626 led_chase();
DrewSchaef 0:bcad524c1856 627 display_ou();
DrewSchaef 0:bcad524c1856 628 led_chase();
DrewSchaef 0:bcad524c1856 629 led_chase();
DrewSchaef 0:bcad524c1856 630 led_chase();
DrewSchaef 0:bcad524c1856 631 display_batt();
DrewSchaef 0:bcad524c1856 632 led_chase();
DrewSchaef 0:bcad524c1856 633 led_chase();
DrewSchaef 0:bcad524c1856 634 led_chase();
DrewSchaef 0:bcad524c1856 635 wait(.5);
DrewSchaef 0:bcad524c1856 636 clear_display();
DrewSchaef 0:bcad524c1856 637 display_mode();
DrewSchaef 0:bcad524c1856 638
DrewSchaef 0:bcad524c1856 639 }
DrewSchaef 0:bcad524c1856 640
DrewSchaef 0:bcad524c1856 641
DrewSchaef 0:bcad524c1856 642 void time_motors()
DrewSchaef 0:bcad524c1856 643 {
DrewSchaef 0:bcad524c1856 644 set_motors(0,0);
DrewSchaef 0:bcad524c1856 645 }
DrewSchaef 0:bcad524c1856 646
DrewSchaef 0:bcad524c1856 647 int main ()
DrewSchaef 0:bcad524c1856 648 {
DrewSchaef 0:bcad524c1856 649
DrewSchaef 0:bcad524c1856 650 Init();
DrewSchaef 0:bcad524c1856 651 //set_motors(1,1);
DrewSchaef 0:bcad524c1856 652 RemoteIR::Format format;
DrewSchaef 0:bcad524c1856 653 uint8_t buf[32];
DrewSchaef 0:bcad524c1856 654 int bitcount = 0;
DrewSchaef 0:bcad524c1856 655 uint8_t fwd= 0;
DrewSchaef 0:bcad524c1856 656 uint8_t rev= 0;
DrewSchaef 0:bcad524c1856 657 uint8_t lft= 0;
DrewSchaef 0:bcad524c1856 658 uint8_t rgt= 0;
DrewSchaef 0:bcad524c1856 659 uint8_t up= 0;
DrewSchaef 0:bcad524c1856 660 uint8_t down= 0;
DrewSchaef 0:bcad524c1856 661
DrewSchaef 0:bcad524c1856 662 while(1) {
DrewSchaef 0:bcad524c1856 663
DrewSchaef 0:bcad524c1856 664 if(mode == 1) {//tv remote mode
DrewSchaef 0:bcad524c1856 665 if(!programmed) {
DrewSchaef 0:bcad524c1856 666 if(!prog_fwd) {
DrewSchaef 0:bcad524c1856 667 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 668 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 669 if(buf[3]!=0) {
DrewSchaef 0:bcad524c1856 670 fwd = buf[3];
DrewSchaef 0:bcad524c1856 671 oled.writeString(2,9,"");
DrewSchaef 0:bcad524c1856 672 oled.printf("%X",fwd);
DrewSchaef 0:bcad524c1856 673 prog_fwd = true;
DrewSchaef 0:bcad524c1856 674 }
DrewSchaef 0:bcad524c1856 675 }
DrewSchaef 0:bcad524c1856 676
DrewSchaef 0:bcad524c1856 677 } else if(!prog_rev) {
DrewSchaef 0:bcad524c1856 678 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 679 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 680 if((buf[3]!=0)&&(buf[3]!=fwd)) {
DrewSchaef 0:bcad524c1856 681 rev = buf[3];
DrewSchaef 0:bcad524c1856 682 oled.writeString(3,9,"");
DrewSchaef 0:bcad524c1856 683 oled.printf("%X",rev);
DrewSchaef 0:bcad524c1856 684 prog_rev = true;
DrewSchaef 0:bcad524c1856 685 }
DrewSchaef 0:bcad524c1856 686 }
DrewSchaef 0:bcad524c1856 687 } else if(!prog_left) {
DrewSchaef 0:bcad524c1856 688 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 689 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 690 if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)) {
DrewSchaef 0:bcad524c1856 691 lft = buf[3];
DrewSchaef 0:bcad524c1856 692 oled.writeString(4,9,"");
DrewSchaef 0:bcad524c1856 693 oled.printf("%X",lft);
DrewSchaef 0:bcad524c1856 694 prog_left = true;
DrewSchaef 0:bcad524c1856 695 }
DrewSchaef 0:bcad524c1856 696 }
DrewSchaef 0:bcad524c1856 697 } else if(!prog_right) {
DrewSchaef 0:bcad524c1856 698 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 699 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 700 if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)) {
DrewSchaef 0:bcad524c1856 701 rgt = buf[3];
DrewSchaef 0:bcad524c1856 702 oled.writeString(5,9,"");
DrewSchaef 0:bcad524c1856 703 oled.printf("%X",rgt);
DrewSchaef 0:bcad524c1856 704 prog_right = true;
DrewSchaef 0:bcad524c1856 705 }
DrewSchaef 0:bcad524c1856 706 }
DrewSchaef 0:bcad524c1856 707 } else if(!prog_up) {
DrewSchaef 0:bcad524c1856 708 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 709 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 710 if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)) {
DrewSchaef 0:bcad524c1856 711 up = buf[3];
DrewSchaef 0:bcad524c1856 712 oled.writeString(6,9,"");
DrewSchaef 0:bcad524c1856 713 oled.printf("%X",up);
DrewSchaef 0:bcad524c1856 714 prog_up = true;
DrewSchaef 0:bcad524c1856 715 }
DrewSchaef 0:bcad524c1856 716 }
DrewSchaef 0:bcad524c1856 717 } else if(!prog_down) {
DrewSchaef 0:bcad524c1856 718 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 719 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 720 if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)&&(buf[3]!=up)) {
DrewSchaef 0:bcad524c1856 721 down = buf[3];
DrewSchaef 0:bcad524c1856 722 oled.writeString(7,9,"");
DrewSchaef 0:bcad524c1856 723 oled.printf("%X",down);
DrewSchaef 0:bcad524c1856 724 prog_down = true;
DrewSchaef 0:bcad524c1856 725 }
DrewSchaef 0:bcad524c1856 726 }
DrewSchaef 0:bcad524c1856 727 }
DrewSchaef 0:bcad524c1856 728 } else {
DrewSchaef 0:bcad524c1856 729
DrewSchaef 0:bcad524c1856 730 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
DrewSchaef 0:bcad524c1856 731 timeout.attach(&time_motors, .1);
DrewSchaef 0:bcad524c1856 732 bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
DrewSchaef 0:bcad524c1856 733 if(buf[3]==fwd) {//forward
DrewSchaef 0:bcad524c1856 734 set_motors(speed,speed);
DrewSchaef 0:bcad524c1856 735
DrewSchaef 0:bcad524c1856 736 } else if(buf[3]==rev) { //reverse
DrewSchaef 0:bcad524c1856 737 set_motors(-speed,-speed);
DrewSchaef 0:bcad524c1856 738
DrewSchaef 0:bcad524c1856 739 } else if(buf[3]==rgt) { //right
DrewSchaef 0:bcad524c1856 740 set_motors(speed*.75,-speed*.75);
DrewSchaef 0:bcad524c1856 741
DrewSchaef 0:bcad524c1856 742 } else if(buf[3]==lft) { //left
DrewSchaef 0:bcad524c1856 743 set_motors(-speed*.75,speed*.75);
DrewSchaef 0:bcad524c1856 744
DrewSchaef 0:bcad524c1856 745 } else if(buf[3]==down) {
DrewSchaef 0:bcad524c1856 746 if(speed > 0.3) {
DrewSchaef 0:bcad524c1856 747 speed = speed-.05;
DrewSchaef 0:bcad524c1856 748 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 749 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 750 }
DrewSchaef 0:bcad524c1856 751
DrewSchaef 0:bcad524c1856 752 } else if(buf[3]==up) {
DrewSchaef 0:bcad524c1856 753 if(speed <1) {
DrewSchaef 0:bcad524c1856 754 speed = speed+.05;
DrewSchaef 0:bcad524c1856 755 oled.writeString(7,0,"speed: ");
DrewSchaef 0:bcad524c1856 756 oled.printf("%.2f",speed);
DrewSchaef 0:bcad524c1856 757 }
DrewSchaef 0:bcad524c1856 758 }
DrewSchaef 0:bcad524c1856 759 }
DrewSchaef 0:bcad524c1856 760
DrewSchaef 0:bcad524c1856 761 }
DrewSchaef 0:bcad524c1856 762 if(pressed) {
DrewSchaef 0:bcad524c1856 763 clear_display();
DrewSchaef 0:bcad524c1856 764 display_mode();
DrewSchaef 0:bcad524c1856 765 pressed = false;
DrewSchaef 0:bcad524c1856 766 }
DrewSchaef 0:bcad524c1856 767 } else if(mode == 2) {//wiimote mode
DrewSchaef 0:bcad524c1856 768 USBLoop();//wait for commands
DrewSchaef 0:bcad524c1856 769 if(pressed) {
DrewSchaef 0:bcad524c1856 770 clear_display();
DrewSchaef 0:bcad524c1856 771 display_mode();
DrewSchaef 0:bcad524c1856 772 pressed = false;
DrewSchaef 0:bcad524c1856 773 }
DrewSchaef 0:bcad524c1856 774 } else if (mode == 3) {//cds
DrewSchaef 0:bcad524c1856 775 //speed = 0.5;
DrewSchaef 0:bcad524c1856 776
DrewSchaef 0:bcad524c1856 777 //if(!calibrated) {
DrewSchaef 0:bcad524c1856 778 uint32_t h_sensor = 0;
DrewSchaef 0:bcad524c1856 779 float cds_l = CDS_L.read();
DrewSchaef 0:bcad524c1856 780 if (cds_l > cds_l_max) {
DrewSchaef 0:bcad524c1856 781 cds_l_max = cds_l;
DrewSchaef 0:bcad524c1856 782 } else if (cds_l < cds_l_min) {
DrewSchaef 0:bcad524c1856 783 cds_l_min = cds_l;
DrewSchaef 0:bcad524c1856 784 }
DrewSchaef 0:bcad524c1856 785
DrewSchaef 0:bcad524c1856 786 float cds_r = CDS_R.read();
DrewSchaef 0:bcad524c1856 787 if (cds_r > cds_r_max) {
DrewSchaef 0:bcad524c1856 788 cds_r_max = cds_r;
DrewSchaef 0:bcad524c1856 789 } else if (cds_r < cds_r_min) {
DrewSchaef 0:bcad524c1856 790 cds_r_min = cds_r;
DrewSchaef 0:bcad524c1856 791 }
DrewSchaef 0:bcad524c1856 792
DrewSchaef 0:bcad524c1856 793 float den_l = cds_l_max - cds_l_min;
DrewSchaef 0:bcad524c1856 794 float den_r = cds_r_max - cds_r_min;
DrewSchaef 0:bcad524c1856 795
DrewSchaef 0:bcad524c1856 796 if(den_l != 0) {
DrewSchaef 0:bcad524c1856 797 cds_l = ((cds_l - cds_l_min)/ den_l);
DrewSchaef 0:bcad524c1856 798 }
DrewSchaef 0:bcad524c1856 799
DrewSchaef 0:bcad524c1856 800 if(den_r != 0) {
DrewSchaef 0:bcad524c1856 801 cds_r = ((cds_r - cds_r_min)/ den_r);
DrewSchaef 0:bcad524c1856 802 }
DrewSchaef 0:bcad524c1856 803
DrewSchaef 0:bcad524c1856 804 int n_loop = (1-cds_l)*32;//normalize to 32 or less (max value of sensor values is 1000)
DrewSchaef 0:bcad524c1856 805 for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
DrewSchaef 0:bcad524c1856 806 h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
DrewSchaef 0:bcad524c1856 807 }
DrewSchaef 0:bcad524c1856 808 oled.fillDisplay((0xFF&h_sensor), 3, 3, 30, 60);//break up that 32 bit integer and look at top 8 bits
DrewSchaef 0:bcad524c1856 809 oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 30, 60);//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 810 oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 30, 60);//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 811 oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 30, 60);//break up that 32 bit integer and look at bottom 8 bits
DrewSchaef 0:bcad524c1856 812 h_sensor = 0;//reset height of sensor bar
DrewSchaef 0:bcad524c1856 813
DrewSchaef 0:bcad524c1856 814 n_loop = (1-cds_r)*32;//normalize to 32 or less (max value of sensor values is 1000)
DrewSchaef 0:bcad524c1856 815 for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
DrewSchaef 0:bcad524c1856 816 h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
DrewSchaef 0:bcad524c1856 817 }
DrewSchaef 0:bcad524c1856 818 oled.fillDisplay((0xFF&h_sensor), 3, 3, 70, 100);//break up that 32 bit integer and look at top 8 bits
DrewSchaef 0:bcad524c1856 819 oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 70, 100);//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 820 oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 70, 100);//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 821 oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 70, 100);//break up that 32 bit integer and look at bottom 8 bits
DrewSchaef 0:bcad524c1856 822 h_sensor = 0;//reset height of sensor bar
DrewSchaef 0:bcad524c1856 823 //}
DrewSchaef 0:bcad524c1856 824 if(calibrated) {
DrewSchaef 0:bcad524c1856 825 float l_speed = (1 - cds_l)*0.5;
DrewSchaef 0:bcad524c1856 826 if(l_speed < 0.25) {
DrewSchaef 0:bcad524c1856 827 l_speed = 0;
DrewSchaef 0:bcad524c1856 828 }
DrewSchaef 0:bcad524c1856 829 float r_speed = (1 - cds_r)*0.5;
DrewSchaef 0:bcad524c1856 830 if(r_speed < 0.25) {
DrewSchaef 0:bcad524c1856 831 r_speed = 0;
DrewSchaef 0:bcad524c1856 832 }
DrewSchaef 0:bcad524c1856 833
DrewSchaef 0:bcad524c1856 834 LeftMotor.speed(l_speed);
DrewSchaef 0:bcad524c1856 835 RightMotor.speed(r_speed);
DrewSchaef 0:bcad524c1856 836 }
DrewSchaef 0:bcad524c1856 837 if(pressed) {
DrewSchaef 0:bcad524c1856 838 //mode = 0;
DrewSchaef 0:bcad524c1856 839 clear_display();
DrewSchaef 0:bcad524c1856 840 display_mode();
DrewSchaef 0:bcad524c1856 841 pressed = false;
DrewSchaef 0:bcad524c1856 842 }
DrewSchaef 0:bcad524c1856 843
DrewSchaef 0:bcad524c1856 844 } else if (mode == 4) {
DrewSchaef 0:bcad524c1856 845 if(!calibrated) {//calibrate
DrewSchaef 0:bcad524c1856 846 qtra.calibrate();
DrewSchaef 0:bcad524c1856 847 qtra.readCalibrated(sensorValues);
DrewSchaef 0:bcad524c1856 848 uint32_t h_sensor = 0;
DrewSchaef 0:bcad524c1856 849
DrewSchaef 0:bcad524c1856 850 //stupid method to make qtra sensitivity bar indication since this oled doesn't fucking have direct pixel indexing (it has page indexing where there are 8 pages(0-7) and each page is made up of 8 rows of pixels)
DrewSchaef 0:bcad524c1856 851 for(int i = 0; i<8; i++) {//loop through 8 sensors
DrewSchaef 0:bcad524c1856 852 int n_loop = sensorValues[i]/32;//normalize to 32 or less (max value of sensor values is 1000)
DrewSchaef 0:bcad524c1856 853 for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
DrewSchaef 0:bcad524c1856 854 h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
DrewSchaef 0:bcad524c1856 855 }
DrewSchaef 0:bcad524c1856 856 oled.fillDisplay((0xFF&h_sensor), 3, 3, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at top 8 bits
DrewSchaef 0:bcad524c1856 857 oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 858 oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at next 8 bits
DrewSchaef 0:bcad524c1856 859 oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at bottom 8 bits
DrewSchaef 0:bcad524c1856 860 h_sensor = 0;//reset height of sensor bar
DrewSchaef 0:bcad524c1856 861 }
DrewSchaef 0:bcad524c1856 862 } else if(!checked) {//check line position
DrewSchaef 0:bcad524c1856 863 int position = qtra.readLine(sensorValues);
DrewSchaef 0:bcad524c1856 864 int pos = (position)/66;
DrewSchaef 0:bcad524c1856 865 oled.writeString(7,0,"pos: ");
DrewSchaef 0:bcad524c1856 866 oled.printf("%i",position);
DrewSchaef 0:bcad524c1856 867 oled.fillDisplay(0xFF,3,6,pos,(pos+20));
DrewSchaef 0:bcad524c1856 868 if(pos>0) {
DrewSchaef 0:bcad524c1856 869 oled.fillDisplay(0x00,3,6,0,pos-1);
DrewSchaef 0:bcad524c1856 870 }
DrewSchaef 0:bcad524c1856 871 if((pos+20)<127) {
DrewSchaef 0:bcad524c1856 872 oled.fillDisplay(0x00,3,6,(pos+20)+1,127);
DrewSchaef 0:bcad524c1856 873 }
DrewSchaef 0:bcad524c1856 874 } else {//start
DrewSchaef 0:bcad524c1856 875
DrewSchaef 0:bcad524c1856 876 int position = qtra.readLine(sensorValues);
DrewSchaef 0:bcad524c1856 877 //printf("%i\n",position);
DrewSchaef 0:bcad524c1856 878 current_position = ((float)position-3500)/3500;
DrewSchaef 0:bcad524c1856 879 proportional = current_position;
DrewSchaef 0:bcad524c1856 880
DrewSchaef 0:bcad524c1856 881 // Compute the derivative
DrewSchaef 0:bcad524c1856 882 derivative = current_position - previous_position;
DrewSchaef 0:bcad524c1856 883
DrewSchaef 0:bcad524c1856 884 // Compute the integral
DrewSchaef 0:bcad524c1856 885 integral += proportional;
DrewSchaef 0:bcad524c1856 886
DrewSchaef 0:bcad524c1856 887
DrewSchaef 0:bcad524c1856 888
DrewSchaef 0:bcad524c1856 889 // Compute the power
DrewSchaef 0:bcad524c1856 890 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
DrewSchaef 0:bcad524c1856 891
DrewSchaef 0:bcad524c1856 892
DrewSchaef 0:bcad524c1856 893 // Compute new speeds
DrewSchaef 0:bcad524c1856 894 right = speed-power;
DrewSchaef 0:bcad524c1856 895 left = speed+power;
DrewSchaef 0:bcad524c1856 896
DrewSchaef 0:bcad524c1856 897 // limit checks
DrewSchaef 0:bcad524c1856 898 if (right < MIN)
DrewSchaef 0:bcad524c1856 899 right = MIN;
DrewSchaef 0:bcad524c1856 900 else if (right > MAX)
DrewSchaef 0:bcad524c1856 901 right = MAX;
DrewSchaef 0:bcad524c1856 902
DrewSchaef 0:bcad524c1856 903 if (left < MIN)
DrewSchaef 0:bcad524c1856 904 left = MIN;
DrewSchaef 0:bcad524c1856 905 else if (left > MAX)
DrewSchaef 0:bcad524c1856 906 left = MAX;
DrewSchaef 0:bcad524c1856 907
DrewSchaef 0:bcad524c1856 908 // set speed
DrewSchaef 0:bcad524c1856 909 set_motors(left,right);
DrewSchaef 0:bcad524c1856 910
DrewSchaef 0:bcad524c1856 911 previous_position = current_position;
DrewSchaef 0:bcad524c1856 912
DrewSchaef 0:bcad524c1856 913 }
DrewSchaef 0:bcad524c1856 914 if(pressed) {
DrewSchaef 0:bcad524c1856 915 clear_display();
DrewSchaef 0:bcad524c1856 916 display_mode();
DrewSchaef 0:bcad524c1856 917 pressed = false;
DrewSchaef 0:bcad524c1856 918 set_motors(0,0);
DrewSchaef 0:bcad524c1856 919 }
DrewSchaef 0:bcad524c1856 920
DrewSchaef 0:bcad524c1856 921 } else if (mode == 6) {
DrewSchaef 0:bcad524c1856 922 if(!set) {
DrewSchaef 0:bcad524c1856 923 set = true;
DrewSchaef 0:bcad524c1856 924 ticker2.attach(&flip,1);
DrewSchaef 0:bcad524c1856 925 }
DrewSchaef 0:bcad524c1856 926 float dist = srf.read();
DrewSchaef 0:bcad524c1856 927 if(dist < 20) {
DrewSchaef 0:bcad524c1856 928 set_motors(.15,.15);
DrewSchaef 0:bcad524c1856 929 } else {
DrewSchaef 0:bcad524c1856 930 if(flipped) {
DrewSchaef 0:bcad524c1856 931 set_motors(.15,-.15);
DrewSchaef 0:bcad524c1856 932 } else {
DrewSchaef 0:bcad524c1856 933 set_motors(0,0);
DrewSchaef 0:bcad524c1856 934 }
DrewSchaef 0:bcad524c1856 935 }
DrewSchaef 0:bcad524c1856 936 wait(.1);
DrewSchaef 0:bcad524c1856 937 if(pressed) {
DrewSchaef 0:bcad524c1856 938 ticker2.detach();
DrewSchaef 0:bcad524c1856 939 clear_display();
DrewSchaef 0:bcad524c1856 940 display_mode();
DrewSchaef 0:bcad524c1856 941 pressed = false;
DrewSchaef 0:bcad524c1856 942 set_motors(0,0);
DrewSchaef 0:bcad524c1856 943 }
DrewSchaef 0:bcad524c1856 944
DrewSchaef 0:bcad524c1856 945 } else if (mode == 5) {
DrewSchaef 0:bcad524c1856 946
DrewSchaef 0:bcad524c1856 947 float dist = srf.read();
DrewSchaef 0:bcad524c1856 948 if(dist > 20) {
DrewSchaef 0:bcad524c1856 949 set_motors(.15,.15);
DrewSchaef 0:bcad524c1856 950 } else {
DrewSchaef 0:bcad524c1856 951 set_motors(-.15,.15);
DrewSchaef 0:bcad524c1856 952 }
DrewSchaef 0:bcad524c1856 953 wait(.1);
DrewSchaef 0:bcad524c1856 954 if(pressed) {
DrewSchaef 0:bcad524c1856 955 clear_display();
DrewSchaef 0:bcad524c1856 956 display_mode();
DrewSchaef 0:bcad524c1856 957 pressed = false;
DrewSchaef 0:bcad524c1856 958 set_motors(0,0);
DrewSchaef 0:bcad524c1856 959 }
DrewSchaef 0:bcad524c1856 960 }
DrewSchaef 0:bcad524c1856 961
DrewSchaef 0:bcad524c1856 962 }
DrewSchaef 0:bcad524c1856 963 }
DrewSchaef 0:bcad524c1856 964
DrewSchaef 0:bcad524c1856 965
DrewSchaef 0:bcad524c1856 966