Revision 4:b5b7836ca2b0, committed 2017-04-28

Comitter:

kyleliangus

Date:

Fri Apr 28 02:13:24 2017 +0000

Parent:

3:880f15be8c72

Child:

5:7e1e4cc19044

Commit message:

Added Modularity, Motors

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/irled.cpp	Fri Apr 28 02:13:24 2017 +0000
@@ -0,0 +1,60 @@
+#include "irled.h"
+#include "mbed.h"
+
+void IrLed::calibrateSensor() {
+
+    for (int i = 0; i < samplesToTake; ++i) 
+        sensorAvg += ir.read();
+        
+    sensorAvg /= samplesToTake;
+}
+
+float IrLed::getSamples( int samples )
+{
+    float z = 0;
+    for( int i = 0; i < samples; ++i )
+        z += ir.read();
+    return z / samples;
+}
+
+float IrLed::blinkLED( int i )
+{
+    return 0.0;
+}
+
+/*
+inline float IrLED::blinkLED( int i, int samples )
+{
+    float z = 0;
+    if( i == 1 )
+    {
+        IR_LED1.write(1);
+        for( int j = 0; j < samples; j++ )
+            z += IR_Sensor1.read();
+        IR_LED1.write(0);
+    }
+    if( i == 2 )
+    {
+        IR_LED2.write(1);
+        for( int j = 0; j < samples; j++ )
+            z += IR_Sensor2.read();
+        IR_LED2.write(0);
+    }
+    if( i == 3 )
+    {
+        IR_LED3.write(1);   
+        for( int j = 0; j < samples; j++ )
+            z += IR_Sensor3.read();
+        IR_LED4.write(0);
+    }
+    if( i == 4 )
+    {
+        IR_LED4.write(1);
+        for( int j = 0; j < samples; j++ )
+            z += IR_Sensor4.read();
+        IR_LED4.write(0);
+    }
+    if( DEBUGGING )
+        serial.println( "Sample by IR %d: %f\n", i, z );
+    return z / samples;
+}*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/irled.h	Fri Apr 28 02:13:24 2017 +0000
@@ -0,0 +1,28 @@
+#ifndef IRLED_H
+#define IRLED_H
+
+#include "mbed.h"
+
+#define samplesToTake 1000
+
+class IrLed
+{
+    public:
+        IrLed( PinName pin ) : ir( pin ) 
+        {
+            calibrateSensor();
+        }
+        
+        float getSamples( int i );
+        
+    private:
+        void calibrateSensor();
+        
+        // internal values
+        DigitalOut ir;
+        float sensorAvg;
+};
+
+
+
+#endif
\ No newline at end of file

--- a/main.cpp	Fri Apr 28 01:08:29 2017 +0000
+++ b/main.cpp	Fri Apr 28 02:13:24 2017 +0000
@@ -1,32 +1,17 @@
 #include "mbed.h"
+
+#include "irled.h"
+#include "main.h"
+#include "motor.h"
+
 #include <stdlib.h>
 #include "ITG3200.h"
 
-
-DigitalOut redLed(PC_0);
-DigitalOut blueLed(PC_1);
-DigitalOut greenLed(PC_2);
-
-DigitalOut IR_1(PB_1);
-DigitalOut IR_2(PB_13);
-DigitalOut IR_3(PB_0);
-DigitalOut IR_4(PB_14);
+// PID
+#define P_CONSTANT 0.0025
+#define I_CONSTANT 0.0000025
+#define D_CONSTANT 0.25
 
-AnalogIn Rec_1(PC_5);
-AnalogIn Rec_2(PC_4);
-AnalogIn Rec_3(PA_6);
-AnalogIn Rec_4(PA_7);
-
-Serial serial(PC_6, PC_7);
-
-ITG3200 gyro(PC_9, PA_8);
-
-
-volatile double reading = 0;
-
-int gyroX = 0;
-int gyroY = 0;
-int gyroZ = 0;
 
 int main()
 {

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Fri Apr 28 02:13:24 2017 +0000
@@ -0,0 +1,43 @@
+#ifndef MAIN_H
+#define MAIN_H
+
+#include "mbed.h"
+#include "ITG3200.h"
+
+// Motors
+PwmOut left1(PB_7);
+PwmOut left2(PB_8);
+PwmOut right1(PA_10);
+PwmOut right2(PA_11);
+
+// RGB LED
+DigitalOut redLed(PC_0);
+DigitalOut blueLed(PC_1);
+DigitalOut greenLed(PC_2);
+
+// IRs
+DigitalOut IR_1(PB_1);
+DigitalOut IR_2(PB_13);
+DigitalOut IR_3(PB_0);
+DigitalOut IR_4(PB_14);
+
+// Receivers
+AnalogIn Rec_1(PC_5);
+AnalogIn Rec_2(PC_4);
+AnalogIn Rec_3(PA_6);
+AnalogIn Rec_4(PA_7);
+
+// Doing DEBUGGING
+#define DEBUGGING 1
+Serial serial(PC_6, PC_7);
+
+// Gyro
+ITG3200 gyro(PC_9, PA_8);
+
+volatile double reading = 0;
+
+int gyroX = 0;
+int gyroY = 0;
+int gyroZ = 0;
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Fri Apr 28 02:13:24 2017 +0000
@@ -0,0 +1,64 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "mbed.h"
+#include "main.h"
+
+#define BRAKE_VOLTAGE 0.2
+
+extern PwmOut left1;
+extern PwmOut left2;
+extern PwmOut right1;
+extern PwmOut right2;
+
+inline void rightWheelForward(double voltage) {
+    right1.write(voltage);
+    right2.write(0);
+}
+
+inline void rightWheelBackward(double voltage) {
+    right1.write(0);
+    right2.write(voltage);
+}
+
+inline void leftWheelForward(double voltage) {
+    left1.write(0);
+    left2.write(voltage);
+}
+
+inline void leftWheelBackward(double voltage) {
+    left1.write(voltage);
+    left2.write(0);
+}
+
+inline void brakeLeftWheel() {
+    left1.write(BRAKE_VOLTAGE);
+    left2.write(BRAKE_VOLTAGE);
+}
+
+inline void brakeRightWheel() {
+    right1.write(BRAKE_VOLTAGE);
+    right2.write(BRAKE_VOLTAGE);
+}
+
+inline void brake() {
+    brakeLeftWheel();
+    brakeRightWheel();
+}
+
+inline void coastLeftWheel() {
+    left1.write(0);
+    left2.write(0);
+}
+
+inline void coastRightWheel() {
+    right1.write(0);
+    right2.write(0);
+}
+
+inline void coast() {
+    coastLeftWheel();
+    coastRightWheel();
+}
+
+#endif
\ No newline at end of file