Rachel Ireland-Jones
/
FinalYear0
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.
Diff: main.cpp
- Revision:
- 18:11937e78239c
- Parent:
- 17:a92d96b65cbc
- Child:
- 19:d3b82416df50
--- a/main.cpp Tue Nov 26 16:34:50 2019 +0000
+++ b/main.cpp Tue Dec 03 16:39:25 2019 +0000
@@ -1,196 +1,256 @@
-//Enhancement 2//
+/*
+Our version
+*/
#include "mbed.h"
+
+//Status LED
+DigitalOut led(LED1);
+
//Motor PWM (speed)
PwmOut PWMA(PA_8);
PwmOut PWMB(PB_4);
+
//Motor Direction
DigitalOut DIRA(PA_9);
DigitalOut DIRB(PB_10);
+
//Hall-Effect Sensor Inputs
DigitalIn HEA1(PB_2);
DigitalIn HEA2(PB_1);
DigitalIn HEB1(PB_15);
DigitalIn HEB2(PB_14);
+
+
//On board switch
DigitalIn SW1(USER_BUTTON);
+
//Use the serial object so we can use higher speeds
Serial terminal(USBTX, USBRX);
+
//Timer used for measuring speeds
-Timer timerA1;
-Timer timerA2;
-Timer timerB1;
-Timer timerB2;
+Timer timerA;
+Timer timerB;
Timer timer1;
+
//Enumerated types
enum DIRECTION {FORWARD=0, REVERSE};
enum PULSE {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};
+
+//Debug GPIO
+DigitalOut probe(D10);
+
//Duty cycles
-float dutyA;
-float dutyB;
+float dutyA = 1.0f; //100%
+float dutyB = 1.0f; //100%
+//Array of sensor data
+int tA1[2];
+int tA2[2];
+int tB1[2];
+int tB2[2];
float dis;
float trav =0;
-float speedA, speedB = 1.0f;
-float paceA, paceB = 1.0f;
-int tmrA1[2];
-int tmrA2[2];
-int tmrB1[2];
-int tmrB2[2];
-void time()
+
+void HallA()
+ {
+ //Reset timer and Start
+ timerA.reset();
+ timerA.start();
+ bool all = true;
+ //**********************
+ //TIME THE FULL SEQUENCE
+ //**********************
+ int HallStateA = 0;
+ while(all)
+ {
+ switch(HallStateA)
+ {
+ case 0:
+ if(HEA1 == NOPULSE){
+ HallStateA = 1;
+ tA1[0] = timerA.read_us();
+ }break;
+ case 1:
+ if(HEA2 == NOPULSE){
+ HallStateA = 2;
+ tA2[0] = timerA.read_us();
+ }break;
+ case 2:
+ if(HEA1 == PULSE){
+ HallStateA = 3;
+ tA1[1] = timerA.read_us();
+ }break;
+ case 3:
+ if(HEA2 == PULSE){
+ HallStateA = 0;
+ all = false;
+ tA2[1] = timerA.read_us();
+ }break;
+ }
+ }
+
+
+ terminal.printf("tA1(0) = %d\n", tA1[0]);
+ terminal.printf("tA1(1) = %d\n", tA1[1]);
+ terminal.printf("tA2(0) = %d\n", tA2[0]);
+ terminal.printf("tA2(1) = %d\n", tA2[1]);
+
+ //Calculate the frequency of rotation
+ float TA1 = 2.0f * (tA1[1]-tA1[0]);
+ float TA2 = 2.0f * (tA2[1]-tA2[0]);
+ float TA = (TA1 + TA2) * 0.5f;
+
+ dis = timer1.read_us();
+ float mm = ((TA*3)*20.8)/175.9;
+ trav = dis/mm;
+ float fA = 1.0f/ (TA *(float)3.0E-6);
+ terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f \t trav: %6.2f\n", fA, fA/20.8f, trav);
+ }
+
+void HallB()
+ {
+ //Reset timer and Start
+ timerB.reset();
+ timerB.start();
+ bool allB = true;
+ //**********************
+ //TIME THE FULL SEQUENCE
+ //**********************
+ int HallStateB = 0;
+ while(allB)
+ {
+ switch(HallStateB)
+ {
+ case 0:
+ if(HEB1 == NOPULSE){
+ HallStateB = 1;
+ tB1[0] = timerB.read_us();
+ }break;
+ case 1:
+ if(HEB2 == NOPULSE){
+ HallStateB = 2;
+ tB2[0] = timerB.read_us();
+ }break;
+ case 2:
+ if(HEB1 == PULSE){
+ HallStateB = 3;
+ tB1[1] = timerB.read_us();
+ }break;
+ case 3:
+ if(HEB2 == PULSE){
+ HallStateB = 0;
+ allB = false;
+ tB2[1] = timerB.read_us();
+ }break;
+ }
+ }
+
+
+ terminal.printf("tB1(0) = %d\n", tB1[0]);
+ terminal.printf("tB1(1) = %d\n", tB1[1]);
+ terminal.printf("tB2(0) = %d\n", tB2[0]);
+ terminal.printf("tB2(1) = %d\n", tB2[1]);
+
+ //Calculate the frequency of rotation
+ float TB1 = 2.0f * (tB1[1]-tB1[0]);
+ float TB2 = 2.0f * (tB2[1]-tB2[0]);
+ float TB = (TB1 + TB2) * 0.5f;
+ float fB = 1.0f/ (TB *(float)3.0E-6);
+ }
+
+void reset()
{
- terminal.printf("tmrA1(0) = %d\n", tmrA1[0]);
- terminal.printf("tmrA1(1) = %d\n", tmrA1[1]);
- terminal.printf("tmrB1(0) = %d\n", tmrA2[0]);
- terminal.printf("tmrB1(1) = %d\n", tmrA2[1]);
- float TA1 = 2.0f * (tmrA1[1]-tmrA1[0]);
- float TA2 = 2.0f * (tmrA2[1]-tmrA2[0]);
- float TA = (TA1 + TA2) * 0.5f;
- float TB1 = 2.0f * (tmrB1[1]-tmrB1[0]);
- float TB2 = 2.0f * (tmrB2[1]-tmrB2[0]);
- float TB = (TB1 + TB2) * 0.5f;
- dis = timer1.read_us();
- float mm = (((((TA*0.3)*20.8)/175.9)+(((TB*0.3)*20.8)/175.9)))*0.5f;
- trav = dis/mm;
- float fA = 1.0f/ (TA *(float)3.0E-7); // FOR A
- float fB = 1.0f/ (TB *(float)3.0E-7); // FOR B
- terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n",fA/20.8f, fB/20.8f, trav);
- speedA = fA/20.8f;
- speedB = fB/20.8f;
+ timer1.reset();
+ HallA();
}
-//Rotational speed towards 1rps
-void oneRPS()
-{
- time();
- if (speedA <= 1.0f)
- {
- paceA +=0.01f;
- wait_ms(25);
- }
- if (speedA >= 1.0f)
- {
- paceA -=0.01f;
- wait_ms(25);
- }
- if (speedB <= 1.0f)
- {
- paceB +=0.01f;
- wait_ms(25);
- }
- if (speedB >= 1.0f)
- {
- paceB -=0.01f;
- wait_ms(25);
- }
- PWMA.write(paceA);
- PWMB.write(paceA);
-}
-
+
int main()
{
- int hallStateA = 0;
- int hallStateB = 0;
+
//Configure the terminal to high speed
- terminal.baud(115200);
+ terminal.baud(115200);
+
//Set initial motor direction
DIRA = FORWARD;
DIRB = FORWARD;
+
//Set motor period to 100Hz
PWMA.period_ms(10);
PWMB.period_ms(10);
+
//Set initial motor speed to stop
PWMA.write(0.0f); //0% duty cycle
PWMB.write(0.0f); //0% duty cycle
+
//Wait for USER button (blue pull-down switch) to start
terminal.puts("Press USER button to start");
+ led = 0;
while (SW1 == RELEASED);
+ led = 1;
+
+ //Set initial motor speed to stop
+ PWMA.write(0.0f); //Set duty cycle (%)
+ PWMB.write(0.0f); //Set duty cycle (%)
+
//Wait - give time to start running
wait(1.0);
- PWMA.write(1.0f); //100% duty cycle
- PWMB.write(1.0f);
timer1.reset();
timer1.start();
+ //Main polling loop
while(1)
{
- int passA1 = HEA1;
- int passA2 = HEA2;
- int passB1 = HEB1;
- int passB2 = HEB2;
- time();
- switch (hallStateA)
- {
- case 0:
- if (passA1 ==1)
- {
- hallStateA = 1;
- timerA1.reset();
- timerA1.start();
- tmrA1[0] = timerA1.read_us();
- }
- break;
- case 1:
- if (passA2 ==1)
- {
- hallStateA = 2;
- timerA2.reset();
- timerA2.start();
- tmrA2[0] = timerA2.read_us();
- }
- break;
- case 2:
- if (passA1 ==0)
- {
- hallStateA = 3;
- tmrA1[1] = timerA1.read_us();
- timerA1.stop();
- }
- break;
- case 3:
- if (passA2 ==0)
- {
- hallStateA = 0;
- tmrA2[1] = timerA2.read_us();
- timerA2.stop();
- }
- break;
- }
-
-
- switch (hallStateB)
- {
- case 0:
- if (passB1 ==1)
- {
- hallStateB = 1;
- timerB1.reset();
- timerB1.start();
- tmrB1[0] = timerB1.read_us();
- }
- break;
- case 1:
- if (passB2 ==1)
- {
- hallStateB = 2;
- timerB2.reset();
- timerB2.start();
- tmrB2[0] = timerB2.read_us();
- }
- break;
- case 2:
- if (passB1 ==0)
- {
- hallStateB = 3;
- tmrB1[1] = timerB1.read_us();
- timerB1.stop();
- }
- break;
- case 3:
- if (passB2 ==0)
- {
- hallStateB = 0;
- tmrB2[1] = timerB2.read_us();
- timerB2.stop();
- }
- break;
- }
+ while(trav <= 1250)
+ {
+ PWMA.write(dutyA); //Set duty cycle y
+ PWMB.write(dutyB);
+ HallA();
+ HallB();
+ }
+ reset();
+ while(trav <= 330)
+ {
+ PWMA.write(dutyA);
+ PWMB.write(0.0f);
+ HallA();
+ HallB();
+ }
+ reset();
+ while(trav <= 1457)
+ {
+ PWMA.write(dutyA);
+ PWMB.write(dutyB);
+ HallA();
+ HallB();
+ }
+ reset();
+ while(trav <= 268)
+ {
+ PWMA.write(dutyA);
+ PWMB.write(0.0f);
+ HallA();
+ HallB();
+ }
+ reset();
+ while(trav <= 750)
+ {
+ PWMA.write(dutyA);
+ PWMB.write(dutyB);
+ HallA();
+ HallB();
+ }
+ reset();
+ while(trav <= 200)
+ {
+ PWMA.write(dutyA);
+ PWMB.write(0.0f);
+ HallA();
+ HallB();
+ }
+ timerA.stop();
+ timerB.stop();
+ break;
}
-}
\ No newline at end of file
+ PWMA.write(0.0f);
+ PWMB.write(0.0f);
+}
+
\ No newline at end of file