Tanmay Bangalore
The IoTBot is a WiFi-enabled rover built from the Shadow Robot kit. It is controlled from a web interface running on the Adafruit Huzzah ESP8266 WiFi module and implements a pair of Hall-effect sensors on the motors to keep the wheels spinning at the same speed. Additionally, it uses a Sharp IR sensor to detect walls in front of the robot and prevent it from crashing.
Dependencies: mbed-dev mbed-rtos
Fork of
huzzah_helloWorld
by 
ECE 4180 Team Who
Diff: main.cpp
- Revision:
- 2:2d87957b577b
- Parent:
- 1:046dd94c57ce
--- a/main.cpp Mon Oct 31 07:33:45 2016 +0000
+++ b/main.cpp Wed Nov 02 17:15:46 2016 +0000
@@ -1,5 +1,6 @@
#include "mbed.h"
#include "rtos.h"
+#include "SongPlayer.h"
RawSerial pc(USBTX, USBRX);
RawSerial dev(p28,p27);
@@ -15,12 +16,27 @@
DigitalOut fwdB(p17);
DigitalOut backB(p16);
Timer t;
-float speed = 0.9f;
+
+// Set up Hall-effect control
+DigitalIn EncR(p13); // right motor
+DigitalIn EncL(p14); // left motor
+int oldEncR = 0; // Previous encoder values
+int oldEncL = 0;
+int ticksR = 0; // Encoder wheel state change counts
+int ticksL = 0;
+int E; // Error between the 2 wheel speeds
+float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
+float speedR = 0; //Same for right wheel
+Ticker Sampler; //Interrupt Routine to sample encoder ticks.
+
+// Distance sensor stuff
+AnalogIn irSensor(p15); // Sensor
+bool emergencyStop = false;
float timeout = 0.05f;
int count,ended;
char buf[256];
-char motorCmd[] = "fwrd";
+char motorCmd[] = "stop";
/**
* Get the reply string from the WiFi module
@@ -73,32 +89,32 @@
fwdB = 1;
backA = 0;
backB = 0;
- motorACtrl = speed;
- motorBCtrl = speed;
+ motorACtrl = speedL;
+ motorBCtrl = speedR;
} else if (strcmp(motorCmd, (char *)"back") == 0) {
stby = 1; // Start H-bridge driver
fwdA = 0;
fwdB = 0;
backA = 1;
backB = 1;
- motorACtrl = speed;
- motorBCtrl = speed;
+ motorACtrl = speedL;
+ motorBCtrl = speedR;
} else if (strcmp(motorCmd, (char *)"left") == 0) {
stby = 1; // Start H-bridge driver
fwdA = 1;
fwdB = 0;
backA = 0;
backB = 1;
- motorACtrl = speed;
- motorBCtrl = speed;
+ motorACtrl = speedL;
+ motorBCtrl = speedR;
} else if (strcmp(motorCmd, (char *)"rght") == 0) {
stby = 1; // Start H-bridge driver
fwdA = 0;
fwdB = 1;
backA = 1;
backB = 0;
- motorACtrl = speed;
- motorBCtrl = speed;
+ motorACtrl = speedL;
+ motorBCtrl = speedR;
} else if (strcmp(motorCmd, (char *)"stop") == 0) {
stby = 1; // Start H-bridge driver
fwdA = 0;
@@ -111,11 +127,36 @@
}
}
+/**
+ * Sample encoders and find error on right wheel. Assume Left wheel is always
+ * correct speed.
+ */
+void sampleEncoder()
+{
+ // Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
+ if(((strcmp(motorCmd, (char *)"fwrd") == 0) ||
+ (strcmp(motorCmd, (char *)"back") == 0)) && (ticksL != 0 || ticksR != 0)) {
+ E = ticksL - ticksR; // Find error
+ pc.printf("Error = %d\n\r", E);
+ speedR = speedR + float(E) / 255.0f; // Assign a scaled increment to the right wheel based on error
+ pc.printf("Updating right motor speed to %f\n\r", speedR);
+ runMotor();
+ }
+ ticksR = 0; //Restart the counters
+ ticksL = 0;
+}
+
void dev_recv()
{
led1 = !led1;
getreply(); // Get the serial message
getMotorCommand(); // Extract the motor command from the message
+ pc.printf(buf);
+ // Check whether we are in emergency stop mode (robot is too close to a wall)
+ if (emergencyStop == true && strcmp(motorCmd, (char *)"back") != 0) {
+ // If so, only allow a "back" command
+ strcpy(motorCmd, "stop");
+ }
runMotor(); // Run the motor based on the command
}
@@ -126,6 +167,44 @@
dev.putc(pc.getc());
}
}
+
+/**
+ * Do distance measurement
+ */
+void distance_thread(void const *args) {
+ SongPlayer speaker(p23);
+ float alert1[2] = {440.0, 0.0};
+ float alert1duration[2] = {0.05, 0.0};
+ float alert2[2] = {880.0, 0.0};
+ float alert2duration[2] = {0.05, 0.0};
+ float alert3[2] = {1760.0, 0.0};
+ float alert3duration[2] = {0.05, 0.0};
+ while(1){
+ float reading = irSensor;
+ pc.printf("Distance: %f\n\r", reading);
+ if (reading > 0.7f) {
+ if (strcmp(motorCmd, (char *)"fwrd") == 0){
+ strcpy(motorCmd, "stop");
+ }
+ emergencyStop = true;
+ runMotor();
+ } else if (reading > 0.65f) {
+ speaker.PlaySong(alert3, alert3duration);
+ emergencyStop = false;
+ Thread::wait(60);
+ } else if (reading > 0.55f) {
+ speaker.PlaySong(alert2, alert2duration);
+ emergencyStop = false;
+ Thread::wait(65);
+ } else if (reading > 0.45f) {
+ speaker.PlaySong(alert1, alert1duration);
+ emergencyStop = false;
+ Thread::wait(70);
+ } else {
+ Thread::wait(20); // Wait 20 ms
+ }
+ }
+}
int main()
{
@@ -138,8 +217,31 @@
// Initialize the motors
motorACtrl.period(0.01f);
motorBCtrl.period(0.01f);
+ speedL = 0.9f;
+ speedR = 0.9f;
+
+ // Initialize hall-effect control
+ Sampler.attach(&sampleEncoder, .1); //Sampler uses sampleEncoder function every 20ms
+ EncL.mode(PullUp); // Use internal pullups
+ EncR.mode(PullUp);
+
+ Thread t0(distance_thread);
while(1) {
- sleep();
+ if((strcmp(motorCmd, (char *)"fwrd") == 0) ||
+ (strcmp(motorCmd, (char *)"back") == 0)) { // Only increment tick values if moving forward or reverse
+ int tmpL = EncL; // Sample the current value of the encoders
+ int tmpR = EncR;
+ pc.printf("Encoder values %d, %d\n\r", tmpL, tmpR);
+ if(tmpL != oldEncL) { // Increment ticks every time the state has switched.
+ ticksL++;
+ oldEncL = tmpL;
+ }
+ if(tmpR != oldEncR) {
+ ticksR++;
+ oldEncR = tmpR;
+ }
+ }
+ wait(0.02f); // Sleep 20 ms
}
}
\ No newline at end of file