Merge branch 'main' into menu-management

CleanIt-SC/Headers/collisionAvoidance.h

@@ -0,0 +1,34 @@
+#ifndef COLLISION_AVOIDANCE_H
+#define COLLISION_AVOIDANCE_H
+#include "proximity.h"
+#include "limit_switch.h"
+#define CLOSEST_ALLOWED_DISTANCE 5
+
+int collisionDetectionInit(void);
+int collisionDetected(void);
+int tooCloseToWall(void);
+
+int collisionDetectionInit() {
+    proximity_init();
+    limit_switch_init();
+}
+
+int collisionDetected() {
+    int state = 0x0;
+    if (!hasNOT_collided(SWITCH_FRONT)) state |= 1;
+    else if (!hasNOT_collided(SWITCH_LEFT)) state |= 2;
+    else if (!hasNOT_collided(SWITCH_RIGHT)) state |= 4;
+    return state;
+}
+
+int tooCloseToWall() {
+    int state = 0x0;
+    double leftSensor,rightSensor;
+    leftSensor = proximity_read_average(8,20);
+    rightSensor = proximity_read_average(9, 20);
+    if (leftSensor < CLOSEST_ALLOWED_DISTANCE) state |= 1;
+    else if (rightSensor < CLOSEST_ALLOWED_DISTANCE) state |= 2;
+    return state;
+}
+
+#endif

CleanIt-SC/Headers/hbridge.h

@@ -4,6 +4,13 @@
 
 static float mod = 41940;
 
+void hbridge_init(void);
+void forward(float);
+void backward(float);
+void rotate_left(float);
+void rotate_right(float);
+void stop(void);
+
 void hbridge_init(void) {
     SIM->SCGC5 |= 0x0800;  /* enable clock to Port C */
     SIM->SCGC5 |= 0x0200;  /* enable clock to Port A */
@@ -56,4 +63,11 @@ void rotate_right(float speed){
     TPM0->CONTROLS[2].CnV = 0;
     TPM0->CONTROLS[1].CnV = mod * speed;
 }
+
+void stop() {
+    TPM0->CONTROLS[5].CnV = 0;
+    TPM0->CONTROLS[4].CnV = 0;
+    TPM0->CONTROLS[2].CnV = 0;
+    TPM0->CONTROLS[1].CnV = 0;
+}
 #endif

CleanIt-SC/Headers/limit_switch.h

@@ -0,0 +1,23 @@
+#ifndef LIMIT_SWITCH_H
+#define LIMIT_SWITCH_H
+#include "MKL25Z4.h"
+#define SWITCH_FRONT 0
+#define SWITCH_LEFT 1
+#define SWITCH_RIGHT 2
+
+void limit_switch_init(void);
+bool hasNOT_collided(int pinNumber); 
+
+void limit_switch_init(void){
+    SIM->SCGC5 |= 0x0800;  /* enable clock to Port C */
+    PORTC->PCR[0] = 0x103; /* PTC0, GPIO, enable pullup*/
+    PORTC->PCR[1] = 0x103; /* PTC1, GPIO, enable pullup*/
+    PORTC->PCR[2] = 0x103; /* PTC2, GPIO, enable pullup*/
+    PTC->PDDR = 0x07; /* make PTC2-0 as input pins */
+}
+
+bool hasNOT_collided(int pinNumber) {
+    return PTC->PDIR & (0x01 << pinNumber);
+}
+
+#endif

CleanIt-SC/Headers/pathFollower.h

@@ -0,0 +1,126 @@
+#ifndef PATHFOLLOWER_H
+#define PATHFOLLOWER_H
+#include "hbridge.h"
+#include "collisionAvoidance.h"
+#include "delays.h"
+#define NORMAL_OPERATION_SPEED 0.1f
+#define ROOMBA_LENGTH_TIME 1.2f
+#define NINETY_DEGREE_TURN_TIME 1.0f
+#define SENSOR_DISTANCE_AT_WALL 3
+#define STOP_TIME 0.5f
+
+void turn90Left(void);
+void turn90Right(void);
+void emptyRoomClean(bool);
+void spillClean(int);
+void batteryTest(void);
+void roomOutline(int);
+
+void turn90Left() {
+    rotate_left(NORMAL_OPERATION_SPEED);
+    delayMs(NINETY_DEGREE_TURN_TIME*1000);
+    stop();
+}
+
+void turn90Right() {
+    rotate_right(NORMAL_OPERATION_SPEED);
+    delayMs(NINETY_DEGREE_TURN_TIME*1000);
+    stop();
+}
+
+/*
+The robot follows a simple, zig zag pattern 
+(long forward, turn right, short forward, turn right, long forward, 
+turn left, short forward, turn left, repeat) until it gets to the 
+end of the room.
+*/
+void emptyRoomClean(bool deep) {
+    bool sensorFacingEnd = 1; //0 left, 1 right
+    while (proximity_read_average(8+sensorFacingEnd, 20) >= SENSOR_DISTANCE_AT_WALL) {
+        forward(NORMAL_OPERATION_SPEED);
+        while (hasNOT_collided(SWITCH_FRONT)) {
+            delayMs(ROOMBA_LENGTH_TIME*1000);
+            if (deep) {
+                stop(); 
+                delayMs(STOP_TIME*1000);
+                forward(NORMAL_OPERATION_SPEED);
+            }
+        }
+        stop();
+        if (sensorFacingEnd) {
+            turn90Right();
+            forward(NORMAL_OPERATION_SPEED);
+            delayMs(ROOMBA_LENGTH_TIME*1000);
+            stop();
+            turn90Right();
+        } else {
+            turn90Left();
+            forward(NORMAL_OPERATION_SPEED);
+            delayMs(ROOMBA_LENGTH_TIME*1000);
+            stop();
+            turn90Left();
+        }
+        sensorFacingEnd = !sensorFacingEnd;
+    }
+    forward(NORMAL_OPERATION_SPEED);
+    while (hasNOT_collided(SWITCH_FRONT)) {}
+    stop();
+}
+
+/*
+The robot does an outward spiral, implemented with 
+an increasing counter, so as to clean a specific area. It stops 
+every roomba length or so to do a more thorough cleaning.
+*/
+void spillClean(int totalTime) {
+    int msLeft = totalTime*1000;
+    int forwardTimeMs = ROOMBA_LENGTH_TIME*1000;
+    int temp;
+    int iterations = 1;
+    while (msLeft > 0) {
+        for (int i=0; i<2; i++) {
+            temp = forwardTimeMs;
+            while (temp > 0) {
+                forward(NORMAL_OPERATION_SPEED);
+                delayMs(ROOMBA_LENGTH_TIME*1000);
+                stop();
+                delayMs(STOP_TIME*1000);
+                temp -= ROOMBA_LENGTH_TIME*1000;
+            }
+            turn90Left();
+        }
+        msLeft -= forwardTimeMs + STOP_TIME*1000*iterations*2 + NINETY_DEGREE_TURN_TIME*1000*2;
+        forwardTimeMs += ROOMBA_LENGTH_TIME*1000;
+        iterations++;
+    } 
+}
+
+void batteryTest() { //The robot goes forward indefinitely.
+    forward(1);
+}
+
+/*
+The robot goes around the room, trying to minimize 
+the measurement of the left proximity sensor and maximize the measurement 
+of the right one. This is done to cover the areas that are usually dirtier 
+after sweeping the floor.
+*/
+void roomOutline(int time) {
+    int msTime = time * 1000;
+    int proximityState;
+    while (msTime > 0) {
+        proximityState = tooCloseToWall();
+        if (proximityState == 1) { //left too close, right not too close
+            forward(NORMAL_OPERATION_SPEED);
+        } else if (proximityState == 0) { //both not too close
+            turn90Left();
+        } else if (proximityState == 2) {
+            turn90Right();
+        }
+        msTime--;
+        delay1ms();
+    }
+    stop();
+}
+
+#endif

CleanIt-SC/Headers/proximity.h

@@ -32,17 +32,17 @@ void proximity_init(void){
 // @param  sensorChannel: int corresponding to ADC channel. Should be 8 or 9.
 // @param  meassurments: measurments to average
 // @returns float in cm
-double proximity_read_average(int sensorChannel, int meassurments){
+double proximity_read_average(int sensorChannel, int measurements){
     int rawValue,sum;
     double valueCm;
     sum = 0;
-    for (int i = 0; i < meassurments; i++){
+    for (int i = 0; i < measurements; i++){
         ADC0->SC1[0] = sensorChannel;
         while(!(ADC0->SC1[0] & 0x80)) { } /* wait for COCO */
         sum = sum + ADC0->R[0];
         delayMs(50);
     }
-    rawValue = sum/meassurments;
+    rawValue = sum/measurements;
     valueCm  = 66.3 -0.0507*rawValue + 0.0000163*pow(rawValue,2) -0.00000000188*pow(rawValue,3);
     return valueCm;
 }