Ivelin Kozarev
/
TDP_main_fork
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.
Dependencies: LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed
Revision 37:3d14df6dec34, committed 2015-03-26
- Comitter:
- Reckstyle
- Date:
- Thu Mar 26 20:13:49 2015 +0000
- Parent:
- 36:a48d57d63630
- Commit message:
- fdsa
Changed in this revision
diff -r a48d57d63630 -r 3d14df6dec34 copy.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/copy.cpp Thu Mar 26 20:13:49 2015 +0000
@@ -0,0 +1,264 @@
+/***
+********** MAIN PROGRAM **********
+
+Sensors are mapped on the global variable sensorsCheckSum,
+which multiplies the sensor number by itself to then decode,
+ which sensors are off and which are on
+ie. if sensor rightright - sensorChecksum = 1*1 = 1
+ if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
+ ...
+***/
+
+#include "mbed.h"
+#include "sensor_measure.h"
+#include "Motor_Driver.h"
+#include "shooter.h"
+
+#define PWM_PERIOD_US 10000
+
+typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
+DigitalOut led(LED_RED);
+DigitalOut ledMode(LED_GREEN);
+Serial HC06(PTE0,PTE1);
+
+void turnLeft ();
+void turnRight ();
+void measureSensors ();
+void printBluetooth();
+void shootFind ();
+
+
+Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
+
+mode driveMode; //declaring the variable for the states
+int sensorsCheckSum; //varibale used for sensors mapping access
+int passedTime1,passedTime2;
+int cursor = 0, k = 0;
+static float normalSpeed = 0.15f;
+
+
+
+int main() {
+ led = 1;//start LED with beginning of main program
+ measureTimer.start();
+ driveMode = REGULAR; //initialise drivemoder
+ sensor_initialise(); // initialise sensor values
+ wait(1); //give time to set up the system
+
+ sensorTimer.start(); // start timer for sensors
+
+ motors.setSteeringMode (NORMAL);
+ motors.disableHardBraking();
+ motors.forward ();
+
+ motors.start ();
+
+ motors.setSpeed (normalSpeed);
+
+ motors.enableSlew();
+ int testOtherCases = 0; //used for control
+ while(1){
+
+ measureSensors();
+ printBluetooth();
+ switch (sensorsCheckSum) {
+ case 94: //continue straight
+ motors.setSpeed (normalSpeed);
+ break;
+ case 104:
+ motors.setSpeed (normalSpeed); //move a little forward
+ measureSensors(); //measure again
+ if (sensorsCheckSum == 104 || sensorsCheckSum == 168) { //confirm the turn
+ turnRight();
+ break;
+ case 158:
+ motors.setSpeed (normalSpeed);
+ measureSensors();
+
+ if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
+ turnLeft();
+ }
+ break;
+ case 168:
+ motors.setSpeed (normalSpeed);
+ measureSensors();
+
+ if (sensorsCheckSum == 168) { //confirm the turn
+ turnRight ();
+ }
+
+ break;
+ case 194:
+ motors.setSpeed (normalSpeed);
+ measureSensors();
+
+ if (sensorsCheckSum == 194) { //confirm the turn
+ turnLeft();
+ }
+ break;
+ default:
+ testOtherCases =1;
+ break;
+ }
+ if (testOtherCases == 1) {
+
+ if (sensorsCheckSum < 96) { //turn left
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.075);
+ }
+ else if (sensorsCheckSum > 96) { // turn right
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.1);
+ }
+
+ }
+
+ motors.setSpeed(0.0); //give time for the system to settle
+ wait(0.2);
+ }
+} //int main
+
+//print statements for debugging
+void printBluetooth() { /
+ //HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
+ //HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
+ //HC06.printf("%i %i %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
+ //HC06.printf("%i %i/n%i %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);
+ //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
+ //HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
+ //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
+}
+
+
+
+void turnRight () {
+
+ ledMode = 0; // put on LED to know we're in this state
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1);
+ motors.setSteeringMode(PIVOT_CW); //pivot for the turn
+ motors.setSpeed (normalSpeed);
+ wait (1);
+ int exit = 0; //used for exiting the function
+ motors.setSteeringMode (NORMAL);
+ while (1) {
+ motors.setSpeed(0.0); //stop and check if the line is reached
+ exit = 0;
+ wait (0.4);
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // if two sensors are black you're on the line
+ motors.setSteeringMode(PIVOT_CW); //rotate back to realign
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSteeringMode(NORMAL); //go back to normal
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+ if (measure(sensorArray[i]) == 0)
+ checkSumLocal += i*i;
+ }
+ if (checkSumLocal == 74) {
+ ledMode = 1; // turn off the LED to signify going out of this state
+ return;
+ }
+ else if (checkSumLocal < 74) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal > 74) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+ }
+
+ }
+
+ }
+ motors.setRightSpeed (normalSpeed*1.7); //increase of the motors to bank left
+ motors.setLeftSpeed (normalSpeed);
+ }
+
+}
+
+void turnLeft () {
+
+ ledMode = 0; // put on LED to know we're in this state
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1.1);
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (1);
+ int exit = 0; //used for exiting the function
+ motors.setSteeringMode (NORMAL);
+ while (1) {
+ motors.setSpeed(0.0); //stop and check if the line is reached
+ exit = 0;
+ for (int i = 0; i < 4; i++) {
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // two sensors are considered on line
+ motors.setSteeringMode(PIVOT_CCW); //pivot
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 0; i < 4; i++) {
+ if (measure(sensorArray[i]) == 1)
+ checkSumLocal += (i+1)*(i+1);
+ }
+ if (checkSumLocal == 20) {
+ led = 0;
+ ledMode = 1; // turn off the LED
+ return;
+ }
+ else if (checkSumLocal > 20) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal < 20) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+
+ }
+
+ }
+
+ }
+ motors.setRightSpeed (normalSpeed);
+ motors.setLeftSpeed (normalSpeed*1.5);
+ wait(0.2);
+ }
+}
+
+void shootFind () { //needs to be implemeted
+}
+
+void measureSensors () {
+ sensorsCheckSum = 0; //zero it when first going into the routine
+ int iterationNumber = NUMBER_SENSORS_REGULAR;
+ if (driveMode == SQUARE) {
+ iterationNumber += NUMBER_SENSORS_SQUARE;
+ }
+ for (int i = 0; i < iterationNumber;i++){
+ if (measure(sensorArray[i]) == 1) {//if sensor is white
+ sensorsCheckSum += (i+1)*(i+1);
+ }
+ }
+ if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
+ oldValues[cursor] = sensorsCheckSum;
+ cursor = (cursor+1)%5;
+ }
+}
diff -r a48d57d63630 -r 3d14df6dec34 main.cpp
--- a/main.cpp Wed Mar 25 11:50:44 2015 +0000
+++ b/main.cpp Thu Mar 26 20:13:49 2015 +0000
@@ -27,6 +27,7 @@
void turnRight ();
void measureSensors ();
void printBluetooth();
+void shootFind ();
Timer measureTimer; //Timer used for measurement
@@ -39,13 +40,15 @@
int oldValues[5] = {0};
int cursor = 0, k = 0;
static float normalSpeed = 0.15f;
+int countRightTurns = 0;
+int countLeftTurns = 0;
int main() {
//setup_counter(1000, 0);
// float frequency = measure_frequency(CHANNEL_1);
- led = 0;//start LED with beginning of main program
+ led = 1;//start LED with beginning of main program
measureTimer.start();
driveMode = REGULAR; //initialise drivemoder
@@ -54,86 +57,131 @@
sensorTimer.start(); // start timer for sensors
- motors.setSteeringMode (NORMAL);
- motors.disableHardBraking();
- motors.forward ();
-
- motors.start ();
- motors.setSpeed (normalSpeed);
-
+ squareOut.period(0.01f); // 0.01 second period
+ squareOut.write(0.5); // 50% duty cycle
+// for (int b = 1; b <7;b++) {
+// shoot(0.4);
+// }
+ shoot (0);
+// motors.setSteeringMode (NORMAL);
+// motors.disableHardBraking();
+// motors.forward ();
+//
+// motors.start ();
+//
+// motors.setSpeed (normalSpeed);
+
//motors.enableSlew();
-
+
// HC06.printf("heello");
int testOtherCases = 0; //used for control
while(1){
- measureSensors();
- //printBluetooth();
+
+ // printBluetooth();
// while (HC06.getc() != 'r') {
// measureSensors();
// printBluetooth();
// wait(1);
// }
+
+
+ measureSensors();
+ printBluetooth();
switch (sensorsCheckSum) {
+
+
case 94:
motors.setSpeed (normalSpeed);
- wait(0.4);
+ wait(0.15);
break;
case 104:
- motors.setSpeed (normalSpeed);
- wait(0.15);
- measureSensors();
-
- if (sensorsCheckSum == 104) { //confirm the turn
-
- wait(0.15);
- measureSensors();
- if (sensorsCheckSum == 104)
- turnRight ();
- }
+ if ( countRightTurns == 1)
+ turnRight();
+ else {
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 104) { //confirm the turn
+
+ wait(0.15);
+ measureSensors();
+ if (sensorsCheckSum == 104)
+ turnRight ();
+ }
+ }
break;
+ case 158:
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
+
+ turnLeft();
+ }
+ break;
case 168:
- motors.setSpeed (normalSpeed);
- wait(0.15);
- measureSensors();
-
- if (sensorsCheckSum == 104) { //confirm the turn
+ if ( countRightTurns == 1)
+ turnRight();
+ else {
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
- wait(0.15);
- measureSensors();
- if (sensorsCheckSum == 104)
- turnRight ();
+ if (sensorsCheckSum == 168) { //confirm the turn
+
+ wait(0.15);
+ measureSensors();
+ if (sensorsCheckSum == 168)
+ turnRight ();
+ }
}
break;
+ case 194:
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 194) { //confirm the turn
+
+
+ turnLeft();
+ }
+ break;
default:
testOtherCases =1;
break;
}
if (testOtherCases == 1) {
+
if (sensorsCheckSum < 96) {
motors.setSpeed (0.0);
- motors.setLeftSpeed(normalSpeed*2);
- wait(0.2);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.075);
}
else if (sensorsCheckSum > 96) {
motors.setSpeed (0.0);
- motors.setRightSpeed(normalSpeed*2);
- wait(0.2);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.1);
}
- }
+ }
+
motors.setSpeed(0.0);
wait(0.2);
+
}
} //int main
void printBluetooth() { //for debugging
HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
- //HC06.printf("%i %i %i %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->state);
+ //HC06.printf("%i %i %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
//HC06.printf("%i %i/n%i %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);
//HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
@@ -143,31 +191,210 @@
void turnRight () {
+
ledMode = 0; // put on LED to know we're in this state
- motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
- wait(0.3);
+ if (countRightTurns == 0 ) {
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1);
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (1);
+ }
+ else {
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1);
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (0.8);
+ }
+
+ int exit = 0; //used for exiting the function
motors.setSpeed(0.0);
- motors.setLeftSpeed (normalSpeed); //turn only the relevant wheel
- wait (1.7);
- int exit = 0; //used for exiting the function
-
- for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+ wait(0.5);
+ motors.setSteeringMode (NORMAL);
+ wait(0.3);
+ while (1) {
motors.setSpeed(0.0); //stop and check if the line is reached
exit = 0;
- if (measure(sensorArray[i]) == 0) {//if sensor is black
- exit ++;
+ wait (0.4);
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // two sensors are considered on line
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+
+ if (measure(sensorArray[i]) == 0)
+ checkSumLocal += i*i;
+ }
+ printBluetooth();
+ if (checkSumLocal == 74) {
+ led = 0;
+ ledMode = 1; // turn off the LED
+ countRightTurns ++;
+ wait(1);
+ return;
+ }
+ if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.6);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+
+ }
+ else if (checkSumLocal < 74) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal > 74) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+ motors.setSpeed(0.0);
+ wait(0.4);
+ }
+
+ }
+
}
- if (exit > 1){ // two sensors are considered on line
- ledMode = 1; // turn off the LED
- return;
- }
- motors.setSpeed (normalSpeed);
+// motors.setSpeed(normalSpeed);
+ motors.setRightSpeed (normalSpeed*1.7);
+ motors.setLeftSpeed (normalSpeed);
wait(0.2);
}
}
-void turnLeft () { //TOBE implemented
+void turnLeft () {
+
+ ledMode = 0; // put on LED to know we're in this state
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1.1);
+ motors.setSteeringMode(PIVOT_CW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (1);
+ int exit = 0; //used for exiting the function
+ motors.setSpeed(0.0);
+ wait(0.5);
+ motors.setSteeringMode (NORMAL);
+ wait(0.3);
+ while (1) {
+ motors.setSpeed(0.0); //stop and check if the line is reached
+ exit = 0;
+ wait (0.4);
+ for (int i = 0; i < 4; i++) {
+
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // two sensors are considered on line
+ motors.setSteeringMode(PIVOT_CW);
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 0; i < 4; i++) {
+
+ if (measure(sensorArray[i]) == 1)
+ checkSumLocal += (i+1)*(i+1);
+ }
+ HC06.printf ("checkSumLocal is %i \n", checkSumLocal);
+ if (checkSumLocal == 20) {
+ led = 0;
+ ledMode = 1; // turn off the LED
+ countLeftTurns ++;
+ wait(1);
+ if (countLeftTurns == 1) {
+ //shootFind ();
+ }
+ return;
+ }
+ if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.6);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+
+ }
+ else if (checkSumLocal > 20) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal < 20) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+ motors.setSpeed(0.0);
+ wait(0.4);
+ }
+
+ }
+
+ }
+// motors.setSpeed(normalSpeed);
+ motors.setRightSpeed (normalSpeed);
+ motors.setLeftSpeed (normalSpeed*1.5);
+ wait(0.2);
+ }
+}
+
+void shootFind () {
+ motors.setSpeed(normalSpeed);
+ wait (1);
+ motors.setSteeringMode (PIVOT_CW);
+ wait (0.8);
+ motors.setSteeringMode (NORMAL);
+
+ while (1) {
+ motors.setSpeed(0.0);
+ wait (0.3);
+ for (int i = 8; i < 11 ; i++) {
+ if (measure(sensorArray[i]) == 2){
+ motors.setSteeringMode (PIVOT_CW);
+ motors.setSpeed(normalSpeed);
+ wait (1.6);
+ motors.setSpeed(normalSpeed);
+ for (int b = 1; b <7;b++) {
+ shoot(0.1*(float)b);
+ }
+ shoot (0);
+ motors.setSteeringMode(NORMAL);
+ // while (1) {
+// motors.setSpeed(0.0);
+// wait (0.4);
+// for (int c =4 ;c <8;c++) {
+// if( measure(sensorArray[i]) == 0){
+// return;
+// }
+// }
+// motors.setSpeed(normalSpeed);
+// motors.setLeftSpeed(normalSpeed*1.3);
+// wait(0.2);
+// }
+ return;
+ }
+
+ }
+ motors.setSpeed (normalSpeed);
+ wait (0.5);
+
+ }
}
void measureSensors () {
diff -r a48d57d63630 -r 3d14df6dec34 sensor_measure.h
--- a/sensor_measure.h Wed Mar 25 11:50:44 2015 +0000
+++ b/sensor_measure.h Thu Mar 26 20:13:49 2015 +0000
@@ -30,7 +30,9 @@
DigitalIn pin_left_left_down(PTE22); //pte22
DigitalIn pin_left_right_up(PTE23); //pte29
DigitalIn pin_left_left_up(PTB2);
-
+DigitalIn pin_center_left (PTC7);
+DigitalIn pin_center_center (PTC0);
+DigitalIn pin_center_right (PTC3);
//timer used by the sensor
Timer sensorTimer;
@@ -56,62 +58,77 @@
sensor_data left_left_down;
sensor_data left_right_up;
sensor_data left_left_up;
+sensor_data center_left;
+sensor_data center_center;
+sensor_data center_right;
// and so on....
-sensor_data *sensorArray [NUMBER_SENSORS_REGULAR]; //array just used for getting value out of the sensors, helps for iteration(see main program)
+sensor_data *sensorArray [NUMBER_SENSORS_REGULAR+3]; //array just used for getting value out of the sensors, helps for iteration(see main program)
//Initialise values of all sensors
void sensor_initialise () {
int arrayBuilder = 0; //integer solely used for populating the array
//right_right
right_right_up.pin = &pin_right_right_up;
- right_right_up.blackValue = 18000*BLACK_THRESHOLD;
+ right_right_up.blackValue = 4200*BLACK_THRESHOLD;
right_right_up.whiteValue = 72000;
right_right_up.grayValue = 0; // 0 for all the non-square sensors
right_right_up.state = 0; //setting all sensors as black at begging
sensorArray [arrayBuilder++] = &right_right_up; //Array goes from rightmost to left, then centre?
right_left_up.pin = &pin_right_left_up;
- right_left_up.blackValue = 11500*BLACK_THRESHOLD;
+ right_left_up.blackValue = 5000*BLACK_THRESHOLD;
right_left_up.whiteValue = 70000;
right_left_up.grayValue = 0;
right_left_up.state = 1;
sensorArray [arrayBuilder++] = &right_left_up;
right_right_down.pin = &pin_right_right_down;
- right_right_down.blackValue = 4000*BLACK_THRESHOLD;
+ right_right_down.blackValue = 900*BLACK_THRESHOLD;
right_right_down.whiteValue = 12000;
right_right_down.grayValue = 0;
right_right_down.state = 0;
sensorArray [arrayBuilder++] = &right_right_down;
right_left_down.pin = &pin_right_left_down;
- right_left_down.blackValue = 2500*BLACK_THRESHOLD;
+ right_left_down.blackValue = 1500*BLACK_THRESHOLD;
right_left_down.whiteValue = 17000;
right_left_down.grayValue = 0;
right_left_down.state = 1;
sensorArray[arrayBuilder++] = &right_left_down;
left_right_down.pin = &pin_left_right_down;
- left_right_down.blackValue = 1800*BLACK_THRESHOLD;
+ left_right_down.blackValue = 1000*BLACK_THRESHOLD;
left_right_down.whiteValue = 14000;
left_right_down.grayValue = 0;
left_right_down.state = 1;
sensorArray [arrayBuilder++] = &left_right_down;
left_left_down.pin = &pin_left_left_down;
- left_left_down.blackValue = 3000*BLACK_THRESHOLD;
+ left_left_down.blackValue = 1000*BLACK_THRESHOLD;
left_left_down.whiteValue = 10000;
left_left_down.grayValue = 0;
left_left_down.state = 0;
sensorArray [arrayBuilder++] = &left_left_down;
left_right_up.pin = &pin_left_right_up;
- left_right_up.blackValue = 9500*BLACK_THRESHOLD; //??? it's a trick ;)
+ left_right_up.blackValue = 5000*BLACK_THRESHOLD; //??? it's a trick ;)
left_right_up.whiteValue = 35000;
left_right_up.grayValue = 0;
left_right_up.state = 1;
sensorArray [arrayBuilder++] = &left_right_up;
left_left_up.pin = &pin_left_left_up;
- left_left_up.blackValue = 12000*BLACK_THRESHOLD;
+ left_left_up.blackValue = 9500*BLACK_THRESHOLD;
left_left_up.whiteValue = 46000;
left_left_up.grayValue = 0;
left_left_up.state = 0;
sensorArray [arrayBuilder++] = &left_left_up;
+ center_left.pin = &pin_center_left;
+ center_left.grayValue = 0;
+ center_left.state = 0;
+ sensorArray [arrayBuilder++] = ¢er_left;
+ center_center.pin = &pin_center_center;
+ center_center.grayValue = 0;
+ center_center.state = 0;
+ sensorArray [arrayBuilder++] = ¢er_center;
+ center_right.pin = &pin_center_right;
+ center_right.grayValue = 0;
+ center_right.state = 0 ;
+ sensorArray [arrayBuilder++] = ¢er_right;
}
int counter1 = 0;
@@ -139,22 +156,22 @@
freq = (1/period); // Convert period (in us) to frequency (Hz).
//pc.printf(" period is %f and freq is %f ", period,freq); // Used for debugging
- // sensor->state = freq;
-// return 0;
+ sensor->state = freq;
+ return 0;
// if (sensor->grayValue != 0 && freq < (sensor->grayValue + 1000) && freq > (sensor.grayValue - 1000)) { //definitely not sure about that!
// //this is a gray value sensor in its gray region
// sensor->state = 2;
// return 2;
// }
- if (freq < (sensor->blackValue)){
- sensor->state = 0; //if it's less than black value it is black
- return 0;
- }
- else {
- sensor->state = 1;
- }
- return 1;
+// if (freq < (sensor->blackValue)){
+// sensor->state = 0; //if it's less than black value it is black
+// return 0;
+// }
+// else {
+// sensor->state = 1;
+// }
+// return 1;
//(freq < sensor->black_value*2)? 1 : 0; //freq
}
#endif
\ No newline at end of file