Andrew Schaeffer
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
Diff: main.cpp
- Revision:
- 0:bcad524c1856
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 01 15:57:59 2017 +0000
@@ -0,0 +1,966 @@
+//by Austin Saunders
+//FA2013 - SP2014
+#include "PowerControl/EthernetPowerControl.h"
+#include "mbed.h"
+#include "ReceiverIR.h"
+#include "TransmitterIR.h"
+#include "USBHost.h"
+#include "Utils.h"
+#include "Motor.h"
+#include "Wiimote.h"
+#include "mbed_logo.h"
+#include "ou_ece_logo.h"
+#include "countdown.h"
+#include "battery.h"
+#include "SSD1308.h"
+#include "MCP3008.h"
+#include "QTR_8A.h"
+#include "PinDetect.h"
+#include "SRF05.h"
+
+//Define Pins
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+SRF05 srf(p30, p29);
+
+ReceiverIR ir_rx_L(p15);//Left TSOP324 38Khz IR Receiver
+ReceiverIR ir_rx_R(p16);//Right TSOP324 38Khz IR Receiver
+
+Motor RightMotor(p22,p25,p26);//Right Motor Driven with SN754410 H-Bridge Motor Driver Chip
+Motor LeftMotor(p21,p23,p24);//Left Motor Driven with SN754410 H-Bridge Motor Driver Chip
+
+AnalogIn VBatt(p17);//100k-100k voltage divider that measures (Battery Voltage)/2
+
+AnalogIn CDS_L(p19);//Left Cadmium Sulfide Cell voltage divider to detect light levels
+AnalogIn CDS_R(p20);//Right Cadmium Sulfide Cell voltage divider to detect light levels
+
+
+PinDetect sw3(p7);//top momentary switch
+PinDetect sw2(p6);//bottom momentary switch
+PinDetect sw1(p5);//middle momentary switch
+
+//DigitalOut LED_ON(p8);//control signal for QTR sensor LEDs
+
+I2C i2c(p9, p10);//used for Seeed 128x64 OLED
+SSD1308 oled = SSD1308(i2c, SSD1308_SA0);//Seeed 128x64 OLED
+
+QTR_8A qtra(p8);
+
+// PID terms
+#define P_TERM 3
+#define I_TERM 0
+#define D_TERM 500
+
+#define MAX .5
+#define MIN 0
+
+Ticker ticker;
+Ticker ticker2;
+
+unsigned int sensorValues[8];//used for QTR
+
+volatile int mode;
+volatile int selection;
+volatile int count;
+volatile float speed;
+Timeout timeout;
+
+volatile bool calibrated;
+volatile bool checked;
+volatile bool pressed;
+
+volatile bool programmed;
+volatile bool prog_fwd;
+volatile bool prog_rev;
+volatile bool prog_left;
+volatile bool prog_right;
+volatile bool prog_up;
+volatile bool prog_down;
+volatile bool flipped;
+volatile bool set;
+
+volatile float cds_l_max;
+volatile float cds_r_max;
+volatile float cds_l_min;
+volatile float cds_r_min;
+
+volatile float right;
+volatile float left;
+volatile float derivative,proportional,integral;
+volatile float power;
+volatile float current_position;
+volatile float previous_position;
+
+void set_motors(float l, float r)
+{
+ LeftMotor.speed(l);
+ RightMotor.speed(r);
+}
+
+
+// this is called by the USB infrastructure when a wii message comes in
+void wii_data(char * data)
+{
+
+ Wiimote wii;
+ wii.decode(data);
+ if( wii.left ) {
+ set_motors(-speed,speed);
+ } else if( wii.right ) {
+ set_motors(speed,-speed);
+ } else if( wii.up ) {
+ set_motors(speed,speed);
+ } else if( wii.down ) {
+ set_motors(-speed,-speed);
+ } else if (wii.plus) {
+ if(speed < .99) {
+ speed = speed + 0.01;
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ }
+ } else if (wii.minus) {
+ if(speed >.26) {
+ speed = speed - 0.01;
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ }
+ } else {
+ set_motors(0,0);
+ }
+ float factor = wii.wheel*.015f;
+
+ float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
+ float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
+
+ if( wii.two ) {
+ set_motors(left_factor*speed,right_factor*speed);
+
+ }
+ if( wii.one) {
+ set_motors(-left_factor*speed,-right_factor*speed);
+ }
+
+}
+
+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)
+{
+ float batt_volt = VBatt.read();
+ batt_volt = batt_volt*6.6;
+ return batt_volt;
+}
+
+float get_batt_perc()
+{
+ float batt_perc = get_batt_volt();
+ batt_perc = batt_perc-4;
+ batt_perc = batt_perc/2;
+ batt_perc = batt_perc*100;
+ if (batt_perc<0) {
+ batt_perc = 0;
+ } else if (batt_perc>100) {
+ batt_perc = 100;
+ }
+ return batt_perc;
+}
+
+//display battery power on OLED
+void display_batt()
+{
+ float batt_perc = get_batt_perc();
+ float batt_volt = get_batt_volt();
+ if(batt_perc >= 100) {
+ oled.writeBitmap((uint8_t*) battery_100p);
+ } else if(batt_perc >= 87.5) {
+ oled.writeBitmap((uint8_t*) battery_87_5p);
+ } else if(batt_perc >= 75) {
+ oled.writeBitmap((uint8_t*) battery_75p);
+ } else if(batt_perc >= 62.5) {
+ oled.writeBitmap((uint8_t*) battery_62_5p);
+ } else if(batt_perc >= 50) {
+ oled.writeBitmap((uint8_t*) battery_50p);
+ } else if(batt_perc >= 37.5) {
+ oled.writeBitmap((uint8_t*) battery_37_5p);
+ } else if(batt_perc >= 25) {
+ oled.writeBitmap((uint8_t*) battery_25p);
+ } else if(batt_perc >= 12.5) {
+ oled.writeBitmap((uint8_t*) battery_12_5p);
+ } else {
+ oled.writeBitmap((uint8_t*) battery_0p);
+ }
+ oled.writeString(0,0,"");
+ oled.printf("%.0f%%",batt_perc);
+ oled.writeString(0,12,"");
+ oled.printf("%1.1fV",batt_volt);
+
+}
+
+
+
+void set_leds(int l1, int l2, int l3, int l4)
+{
+ led1 = l1;
+ led2 = l2;
+ led3 = l3;
+ led4 = l4;
+}
+
+void led_chase()
+{
+ set_leds(1,0,0,0);
+ wait(.05);
+ set_leds(0,1,0,0);
+ wait(.05);
+ set_leds(0,0,1,0);
+ wait(.05);
+ set_leds(0,0,0,1);
+ wait(.05);
+ set_leds(0,0,0,0);
+}
+
+void display_ou()
+{
+ oled.writeBitmap((uint8_t*) OU);
+}
+
+void display_mbed()
+{
+ oled.writeBitmap((uint8_t*) mbed_logo);
+}
+
+void display_off()
+{
+ oled.setDisplayOff();
+}
+
+void clear_display()
+{
+ oled.clearDisplay();
+}
+
+void display_mode()
+{
+ if(mode == 0) { //main menu
+ if(selection == 1) {
+ oled.setInverted(true);
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.setInverted(false);
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.writeString(3,0,"Line Following");
+ oled.writeString(4,0,"Object Avoidance");
+ oled.writeString(5,0,"Object Seeking");
+ oled.writeString(6,0,"Display Battery");
+ } else if(selection == 2) {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.setInverted(true);
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.setInverted(false);
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.writeString(3,0,"Line Following");
+ oled.writeString(4,0,"Object Avoidance");
+ oled.writeString(5,0,"Object Seeking");
+ oled.writeString(6,0,"Display Battery");
+ } else if(selection == 3) {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.setInverted(true);
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.setInverted(false);
+ oled.writeString(3,0,"Line Following");
+ oled.writeString(4,0,"Object Avoidance");
+ oled.writeString(5,0,"Object Seeking");
+ oled.writeString(6,0,"Display Battery");
+ } else if(selection == 4) {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.setInverted(true);
+ oled.writeString(3,0,"Line Following");
+ oled.setInverted(false);
+ oled.writeString(4,0,"Object Avoidance");
+ oled.writeString(5,0,"Object Seeking");
+ oled.writeString(6,0,"Display Battery");
+ } else if(selection == 5) {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.writeString(3,0,"Line Following");
+ oled.setInverted(true);
+ oled.writeString(4,0,"Object Avoidance");
+ oled.setInverted(false);
+ oled.writeString(5,0,"Object Seeking");
+ oled.writeString(6,0,"Display Battery");
+ } else if(selection == 6) {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.writeString(3,0,"Line Following");
+ oled.writeString(4,0,"Object Avoidance");
+ oled.setInverted(true);
+ oled.writeString(5,0,"Object Seeking");
+ oled.setInverted(false);
+ oled.writeString(6,0,"Display Battery");
+ } else {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(1,0,"Wiimote Ctrl");
+ oled.writeString(2,0,"Flash Light Ctrl");
+ oled.writeString(3,0,"Line Following");
+ oled.writeString(4,0,"Object Avoidance");
+ oled.writeString(5,0,"Object Seeking");
+ oled.setInverted(true);
+ oled.writeString(6,0,"Display Battery");
+ oled.setInverted(false);
+ }
+ } else if(mode == 1) {
+
+ if(!programmed) {
+ oled.writeString(0,0,"Program");
+ oled.writeString(2,0,"Forward:");
+ oled.writeString(3,0,"Reverse:");
+ oled.writeString(4,0,"Left:");
+ oled.writeString(5,0,"Right:");
+ oled.writeString(6,0,"Speed Up:");
+ oled.writeString(7,0,"Speed Dn:");
+ oled.writeString(0,12,"next");
+ oled.writeString(7,12,"back");
+ } else {
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(7,12,"back");
+ oled.writeBigChar(3,55,'3');
+ wait(1);
+ oled.writeBigChar(3,55,'2');
+ wait(1);
+ oled.writeBigChar(3,55,'1');
+ wait(1);
+ oled.writeBitmap((uint8_t*) big_GO);
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(7,12,"back");
+ led_chase();
+ led_chase();
+ led_chase();
+ clear_display();
+ oled.writeString(0,0,"TV Remote Ctrl");
+ oled.writeString(7,12,"back");
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ }
+ } else if(mode == 2) {
+ oled.writeString(0,0,"Wiimote Ctrl");
+ oled.writeString(7,12,"back");
+
+ } else if(mode == 3) {
+ if(!calibrated) {
+ oled.writeString(0,0,"Calibrate");
+ oled.writeString(1,0,"Sensors");
+ oled.writeString(0,12,"next");
+ oled.writeString(7,12,"back");
+ } else {
+ oled.writeString(0,0,"Flash Light Ctrl");
+ oled.writeString(7,12,"back");
+ oled.writeBigChar(3,55,'3');
+ wait(1);
+ oled.writeBigChar(3,55,'2');
+ wait(1);
+ oled.writeBigChar(3,55,'1');
+ wait(1);
+ oled.writeBitmap((uint8_t*) big_GO);
+ oled.writeString(0,0,"Flash Light Ctrl!");
+ oled.writeString(7,12,"back");
+ led_chase();
+ led_chase();
+ led_chase();
+ clear_display();
+ oled.writeString(0,0,"Flash Light Ctrl!");
+ oled.writeString(7,12,"back");
+ }
+ } else if(mode == 4) {
+ if(!calibrated) {
+ oled.writeString(0,0,"Calibrate");
+ oled.writeString(1,0,"Sensors");
+ oled.writeString(0,12,"next");
+ oled.writeString(7,12,"back");
+ } else if (!checked) {
+ oled.writeString(0,0,"Check");
+ oled.writeString(1,0,"Calibration");
+ oled.writeString(0,12,"next");
+ oled.writeString(7,12,"back");
+ } else {
+ oled.writeString(0,0,"Line Following!");
+ oled.writeString(7,12,"back");
+ oled.writeBigChar(3,55,'3');
+ wait(1);
+ oled.writeBigChar(3,55,'2');
+ wait(1);
+ oled.writeBigChar(3,55,'1');
+ wait(1);
+ oled.writeBitmap((uint8_t*) big_GO);
+ oled.writeString(0,0,"Line Following!");
+ oled.writeString(7,12,"back");
+ led_chase();
+ led_chase();
+ led_chase();
+ }
+ } else if(mode == 5) {
+ oled.writeString(0,0,"Object Avoidance");
+ oled.writeString(7,12,"back");
+ } else if(mode == 6) {
+ oled.writeString(0,0,"Object Seeking");
+ oled.writeString(7,12,"back");
+ } else if(mode == 7) {
+ display_batt();
+ oled.writeString(7,12,"back");
+ }
+
+}
+
+
+
+
+void sw3_pressed()//top switch
+{
+ if(mode == 0) {
+ if(selection>1) {
+ selection = selection-1;
+ } else {
+ selection = 1;
+ }
+ display_mode();
+ } else if(mode == 1) {
+ if (!programmed) {
+ programmed = true;
+ }
+ pressed = true;
+ } else if((mode == 4)||(mode == 3)) {
+ if (!calibrated) {
+ calibrated = true;
+ } else if (!checked) {
+ checked = true;
+ } else {
+
+ }
+ //clear_display();
+ //display_mode();
+ pressed = true;
+
+ }
+}
+
+
+void sw1_pressed()//middle switch
+{
+ if(mode==0) {
+
+ mode = selection;
+ selection = 1;
+ clear_display();
+ display_mode();
+ }
+}
+
+void sw2_pressed()//bottom switch
+{
+ if(mode ==0) {
+ if(selection<7) {
+ selection = selection+1;
+ } else {
+ selection = 7;
+ }
+ display_mode();
+ } else if(mode == 1) {
+ if (programmed) {
+ programmed = false;
+ prog_fwd = false;
+ prog_rev = false;
+ prog_left = false;
+ prog_right = false;
+ prog_up = false;
+ prog_down = false;
+ } else {
+ mode = 0;
+ }
+ pressed = true;
+ } else if(mode == 2) {
+ pressed = true;
+ mode = 0;
+ } else if((mode == 4)||(mode ==3)) {
+ if (checked) {
+ checked = false;
+ set_motors(0,0);
+ } else if (calibrated) {
+ calibrated = false;
+ set_motors(0,0);
+ if (mode ==3) {
+ qtra.resetCalibration();
+ } else {
+ cds_r_max = 0;
+ cds_r_min = 1;
+ cds_l_max = 0;
+ cds_l_min = 1;
+ }
+
+ } else {
+ mode = 0;
+ if (mode ==3) {
+ qtra.resetCalibration();
+ } else {
+ cds_r_max = 0;
+ cds_r_min = 1;
+ cds_l_max = 0;
+ cds_l_min = 1;
+ }
+
+
+ }
+ //clear_display();
+ //display_mode();
+ pressed = true;
+
+ } else if( (mode==5)||(mode==6)) {
+ if (set) {
+ set = false;
+ }
+ mode = 0;
+ pressed = true;
+
+ } else {
+
+ mode = 0;
+ clear_display();
+ display_mode();
+ pressed = true;
+
+ }
+}
+
+void flash()
+{
+ led1 = !led1;
+ led2 = !led2;
+ led3 = !led3;
+ led4 = !led4;
+}
+
+void flip()
+{
+ flipped = !flipped;
+}
+
+void connect()
+{
+ oled.writeString(3,0,"Connect Wiimote");
+}
+
+void connecting()
+{
+ oled.writeString(3,0,"Connecting.....");
+ ticker.attach(&flash,.125);
+}
+
+void connected()
+{
+ oled.writeString(3,0,"Connected! ");
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ set_leds(0,0,0,1);
+ ticker.detach();
+}
+
+
+void Init()
+{
+ PHY_PowerDown();
+ i2c.frequency(400000);//max freq
+ USBInit();
+ set_leds(0,0,0,0);
+ set_motors(0,0);
+ selection = 1;
+ mode = 0;
+ count = 0;
+ speed = MAX;
+ pressed = 0;
+
+ cds_l_max = 0;
+ cds_r_max = 0;
+ cds_l_min = 1;
+ cds_r_min = 1;
+
+ calibrated = false;
+ checked = false;
+ programmed = false;
+ prog_fwd = false;
+ prog_rev = false;
+ prog_left = false;
+ prog_right = false;
+ prog_up = false;
+ prog_down = false;
+ flipped = false;
+ set = false;
+
+ right = 0;
+ left = 0;
+ derivative = 0;
+ proportional = 0;
+ integral = 0;
+ power = 0;
+ current_position = 0;
+ previous_position = 0;
+
+ sw1.mode( PullUp );
+ sw1.attach_deasserted( &sw1_pressed);
+ sw1.setSampleFrequency();
+
+ sw2.mode( PullUp );
+ sw2.attach_deasserted( &sw2_pressed);
+ sw2.setSampleFrequency();
+
+ sw3.mode( PullUp );
+ sw3.attach_deasserted( &sw3_pressed);
+ sw3.setSampleFrequency();
+ oled.setContrastControl(0);
+ display_mbed();
+ led_chase();
+ led_chase();
+ led_chase();
+ display_ou();
+ led_chase();
+ led_chase();
+ led_chase();
+ display_batt();
+ led_chase();
+ led_chase();
+ led_chase();
+ wait(.5);
+ clear_display();
+ display_mode();
+
+}
+
+
+void time_motors()
+{
+ set_motors(0,0);
+}
+
+int main ()
+{
+
+ Init();
+ //set_motors(1,1);
+ RemoteIR::Format format;
+ uint8_t buf[32];
+ int bitcount = 0;
+ uint8_t fwd= 0;
+ uint8_t rev= 0;
+ uint8_t lft= 0;
+ uint8_t rgt= 0;
+ uint8_t up= 0;
+ uint8_t down= 0;
+
+ while(1) {
+
+ if(mode == 1) {//tv remote mode
+ if(!programmed) {
+ if(!prog_fwd) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if(buf[3]!=0) {
+ fwd = buf[3];
+ oled.writeString(2,9,"");
+ oled.printf("%X",fwd);
+ prog_fwd = true;
+ }
+ }
+
+ } else if(!prog_rev) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if((buf[3]!=0)&&(buf[3]!=fwd)) {
+ rev = buf[3];
+ oled.writeString(3,9,"");
+ oled.printf("%X",rev);
+ prog_rev = true;
+ }
+ }
+ } else if(!prog_left) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)) {
+ lft = buf[3];
+ oled.writeString(4,9,"");
+ oled.printf("%X",lft);
+ prog_left = true;
+ }
+ }
+ } else if(!prog_right) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)) {
+ rgt = buf[3];
+ oled.writeString(5,9,"");
+ oled.printf("%X",rgt);
+ prog_right = true;
+ }
+ }
+ } else if(!prog_up) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)) {
+ up = buf[3];
+ oled.writeString(6,9,"");
+ oled.printf("%X",up);
+ prog_up = true;
+ }
+ }
+ } else if(!prog_down) {
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)&&(buf[3]!=up)) {
+ down = buf[3];
+ oled.writeString(7,9,"");
+ oled.printf("%X",down);
+ prog_down = true;
+ }
+ }
+ }
+ } else {
+
+ if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
+ timeout.attach(&time_motors, .1);
+ bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
+ if(buf[3]==fwd) {//forward
+ set_motors(speed,speed);
+
+ } else if(buf[3]==rev) { //reverse
+ set_motors(-speed,-speed);
+
+ } else if(buf[3]==rgt) { //right
+ set_motors(speed*.75,-speed*.75);
+
+ } else if(buf[3]==lft) { //left
+ set_motors(-speed*.75,speed*.75);
+
+ } else if(buf[3]==down) {
+ if(speed > 0.3) {
+ speed = speed-.05;
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ }
+
+ } else if(buf[3]==up) {
+ if(speed <1) {
+ speed = speed+.05;
+ oled.writeString(7,0,"speed: ");
+ oled.printf("%.2f",speed);
+ }
+ }
+ }
+
+ }
+ if(pressed) {
+ clear_display();
+ display_mode();
+ pressed = false;
+ }
+ } else if(mode == 2) {//wiimote mode
+ USBLoop();//wait for commands
+ if(pressed) {
+ clear_display();
+ display_mode();
+ pressed = false;
+ }
+ } else if (mode == 3) {//cds
+ //speed = 0.5;
+
+ //if(!calibrated) {
+ uint32_t h_sensor = 0;
+ float cds_l = CDS_L.read();
+ if (cds_l > cds_l_max) {
+ cds_l_max = cds_l;
+ } else if (cds_l < cds_l_min) {
+ cds_l_min = cds_l;
+ }
+
+ float cds_r = CDS_R.read();
+ if (cds_r > cds_r_max) {
+ cds_r_max = cds_r;
+ } else if (cds_r < cds_r_min) {
+ cds_r_min = cds_r;
+ }
+
+ float den_l = cds_l_max - cds_l_min;
+ float den_r = cds_r_max - cds_r_min;
+
+ if(den_l != 0) {
+ cds_l = ((cds_l - cds_l_min)/ den_l);
+ }
+
+ if(den_r != 0) {
+ cds_r = ((cds_r - cds_r_min)/ den_r);
+ }
+
+ int n_loop = (1-cds_l)*32;//normalize to 32 or less (max value of sensor values is 1000)
+ 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)
+ h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
+ }
+ oled.fillDisplay((0xFF&h_sensor), 3, 3, 30, 60);//break up that 32 bit integer and look at top 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 30, 60);//break up that 32 bit integer and look at next 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 30, 60);//break up that 32 bit integer and look at next 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 30, 60);//break up that 32 bit integer and look at bottom 8 bits
+ h_sensor = 0;//reset height of sensor bar
+
+ n_loop = (1-cds_r)*32;//normalize to 32 or less (max value of sensor values is 1000)
+ 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)
+ h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
+ }
+ oled.fillDisplay((0xFF&h_sensor), 3, 3, 70, 100);//break up that 32 bit integer and look at top 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 70, 100);//break up that 32 bit integer and look at next 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 70, 100);//break up that 32 bit integer and look at next 8 bits
+ oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 70, 100);//break up that 32 bit integer and look at bottom 8 bits
+ h_sensor = 0;//reset height of sensor bar
+ //}
+ if(calibrated) {
+ float l_speed = (1 - cds_l)*0.5;
+ if(l_speed < 0.25) {
+ l_speed = 0;
+ }
+ float r_speed = (1 - cds_r)*0.5;
+ if(r_speed < 0.25) {
+ r_speed = 0;
+ }
+
+ LeftMotor.speed(l_speed);
+ RightMotor.speed(r_speed);
+ }
+ if(pressed) {
+ //mode = 0;
+ clear_display();
+ display_mode();
+ pressed = false;
+ }
+
+ } else if (mode == 4) {
+ if(!calibrated) {//calibrate
+ qtra.calibrate();
+ qtra.readCalibrated(sensorValues);
+ uint32_t h_sensor = 0;
+
+ //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)
+ for(int i = 0; i<8; i++) {//loop through 8 sensors
+ int n_loop = sensorValues[i]/32;//normalize to 32 or less (max value of sensor values is 1000)
+ 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)
+ h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
+ }
+ 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
+ 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
+ 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
+ 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
+ h_sensor = 0;//reset height of sensor bar
+ }
+ } else if(!checked) {//check line position
+ int position = qtra.readLine(sensorValues);
+ int pos = (position)/66;
+ oled.writeString(7,0,"pos: ");
+ oled.printf("%i",position);
+ oled.fillDisplay(0xFF,3,6,pos,(pos+20));
+ if(pos>0) {
+ oled.fillDisplay(0x00,3,6,0,pos-1);
+ }
+ if((pos+20)<127) {
+ oled.fillDisplay(0x00,3,6,(pos+20)+1,127);
+ }
+ } else {//start
+
+ int position = qtra.readLine(sensorValues);
+ //printf("%i\n",position);
+ current_position = ((float)position-3500)/3500;
+ proportional = current_position;
+
+ // Compute the derivative
+ derivative = current_position - previous_position;
+
+ // Compute the integral
+ integral += proportional;
+
+
+
+ // Compute the power
+ power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+
+
+ // Compute new speeds
+ right = speed-power;
+ left = speed+power;
+
+ // limit checks
+ if (right < MIN)
+ right = MIN;
+ else if (right > MAX)
+ right = MAX;
+
+ if (left < MIN)
+ left = MIN;
+ else if (left > MAX)
+ left = MAX;
+
+ // set speed
+ set_motors(left,right);
+
+ previous_position = current_position;
+
+ }
+ if(pressed) {
+ clear_display();
+ display_mode();
+ pressed = false;
+ set_motors(0,0);
+ }
+
+ } else if (mode == 6) {
+ if(!set) {
+ set = true;
+ ticker2.attach(&flip,1);
+ }
+ float dist = srf.read();
+ if(dist < 20) {
+ set_motors(.15,.15);
+ } else {
+ if(flipped) {
+ set_motors(.15,-.15);
+ } else {
+ set_motors(0,0);
+ }
+ }
+ wait(.1);
+ if(pressed) {
+ ticker2.detach();
+ clear_display();
+ display_mode();
+ pressed = false;
+ set_motors(0,0);
+ }
+
+ } else if (mode == 5) {
+
+ float dist = srf.read();
+ if(dist > 20) {
+ set_motors(.15,.15);
+ } else {
+ set_motors(-.15,.15);
+ }
+ wait(.1);
+ if(pressed) {
+ clear_display();
+ display_mode();
+ pressed = false;
+ set_motors(0,0);
+ }
+ }
+
+ }
+}
+
+
+