diff --git a/examples/ms_line_sensor_array_detect_crossing/ms_line_sensor_array_detect_crossing.ino b/examples/ms_line_sensor_array_detect_crossing/ms_line_sensor_array_detect_crossing.ino new file mode 100644 index 0000000..f5b4d4c --- /dev/null +++ b/examples/ms_line_sensor_array_detect_crossing/ms_line_sensor_array_detect_crossing.ino @@ -0,0 +1,112 @@ +/** + * In this example the "Result" value of the Mindsensors Line Sensor Array is + * used to calculate the width of a line e.g. to detect crossings. + * + * Bit operators are used to count the number of Bits set in Result. + * + * In a loop for each Bit in Result a Bit mask is created using a Bit shift + * operation: + * + * 1 << n + * + * For 1 << 0 the Bit mask is: + * + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 1 | + * +---+---+---+---+---+---+---+---+ + * + * For 1 << 3 the Bit mask is: + * + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 1 | 0 | 0 | 0 | + * +---+---+---+---+---+---+---+---+ + * + * For 1 << 7 the Bit mask is: + * + * +---+---+---+---+---+---+---+---+ + * | 1 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | + * +---+---+---+---+---+---+---+---+ + * + * These 8 Bitmasks are each compared with the value of Result using a bitwise + * AND operation (result & bitmask). + * + * This operation performs a logical AND operation on each pair of the + * corresponding bits. If both Bits in a compared position are 1, the Bit in + * the result of this operation is 1. If at least one of the Bits is 0, the Bit + * in the result will be 0. + * + * Given a Result with 4th, 5th and 6th Bit set: + * + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 1 | 1 | 1 | 0 | 0 | Result + * +---+---+---+---+---+---+---+---+ + * bitwise AND + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 1 | (1 << 0) -> 0 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 0 | 0 | 1 | 0 | (1 << 1) -> 0 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 0 | 1 | 0 | 0 | (1 << 2) -> 1 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 0 | 1 | 0 | 0 | 0 | (1 << 3) -> 1 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 0 | 1 | 0 | 0 | 0 | 0 | (1 << 4) -> 1 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 0 | 1 | 0 | 0 | 0 | 0 | 0 | (1 << 5) -> 0 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 0 | 1 | 0 | 0 | 0 | 0 | 0 | 0 | (1 << 6) -> 0 + * +---+---+---+---+---+---+---+---+ + * +---+---+---+---+---+---+---+---+ + * | 1 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | (1 << 7) -> 0 + * +---+---+---+---+---+---+---+---+ + * + * Each time the result of the bitwise AND operation is greater 0 the Bit is + * counted. In the above example the counted number is 3. + * + * A crossing may be detected if the number Bits is significantly greater than + * the number of sensors covered by the line. Note, that the number of sensors + * covered by the line is also greater if the Line Sensor is at an angle to the + * line. + */ +#include +#include + +MsLineSensorArray lsa = MsLineSensorArray(); + +void setup() { + Serial.begin(9600); +} + +void loop() { + int result, bitmask, counter = 0; + + result = lsa.getResult(); + + for (int n = 0; n < 8; n++) { + /* shift a Bit by n places to the left */ + bitmask = 1 << n; + + /* perform a logical AND operation on each Bit */ + if ((result & bitmask) > 0) { + /* if the n-th Bit in result is set, increment counter by one */ + counter++; + } + } + + Serial.print("Line width: "); + Serial.println(counter); + + /** + * if the line normally covers 2 sensors, a counter of 5 or greater is most + * likely indicating a crossing + */ + if (counter > 4) { + Serial.println("Looks like a crossing!"); + } +} diff --git a/examples/ms_line_sensor_array_follow_line/ms_line_sensor_array_follow_line.ino b/examples/ms_line_sensor_array_follow_line/ms_line_sensor_array_follow_line.ino new file mode 100644 index 0000000..61589a6 --- /dev/null +++ b/examples/ms_line_sensor_array_follow_line/ms_line_sensor_array_follow_line.ino @@ -0,0 +1,88 @@ +/** + * In this example the "Average" value of the Mindsensors Line Sensor Array is + * used to calculate a correction value for motor speeds to keep the robot on + * the line. + * + * Edge cases are Average values of + * + * 10 -> Line Sensor is at RIGHT edge of the line + * 80 -> Line Sensor is at LEFT edge of the line + * + * The ideal Average value is: + * + * 45 -> Line Sensor is in the MIDDLE of the line + * + * If average is 80 the robot must turn right. To do so it must + * + * -> INCREASE the speed of the LEFT motor and/or + * -> DECREASE the speed of the RIGHT motor. + * + * If average is 10 the robot must turn left. To do so it must + + * -> DECREASE the speed of the LEFT motor and/or + * -> INCREASE the speed of the RIGHT motor. + * + * Calculate a deviation of the ideal value of 45 using the Average (avg): + * + * deviation = 45 - avg + * + * For 80 it is -> 45 - 80 = -35 + * For 10 it is -> 45 - 10 = 35 + * For 45 it is -> 45 - 45 = 0 + * + * Add these results to the right motor speed, and substract it from the + * left motor speed and you will get the desired results. + */ +#include +#include +#include + +EncoderOnBoardMotor motor_l = EncoderOnBoardMotor(SLOT2); +EncoderOnBoardMotor motor_r = EncoderOnBoardMotor(SLOT1); +MsLineSensorArray lsa = MsLineSensorArray(); + +/* "average" value returned by the sensor */ +int avg; +/* last valid "average" value */ +int last = 45; +/* normal forward speed */ +float normspeed = 50; + +void setup() { +} + +void loop() { + float deviation; + + avg = lsa.getAverage(); + + if (avg != 0) { + /* if avg is valid, safe it for bad times */ + last = avg; + } else { + /** + * if avg is 0 we lost the line so best option is to keep on going with the + * last valid value we got + */ + avg = last; + } + + /* See top comment for details */ + deviation = 45 - avg; + /** + * scale deviaton according to normal speed so it will at most + * + * - decrease to 0 + * - increase to double normspeed + */ + deviation = (deviation / 35.) * normspeed; + /** + * This works only if the line leader is built in in forward direction. + * In forward direction the socket points to the rear of the robot. + * + * If the sensor is built in reverse direction, substract deviation from the + * speed of the right motor and add it to the speed of the left instead. + */ + motor_r.forward(-(normspeed + deviation)); + motor_l.forward(normspeed - deviation); +}