Rachel Ireland-Jones / Mbed OS 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.

main.cpp@16:afe936377d66, 2019-11-20 (annotated)

Committer:
Mikebob
Date:
Wed Nov 20 14:40:26 2019 +0000
Revision:
16:afe936377d66
Parent:
15:b7ca53716b05
Child:
17:a92d96b65cbc
doesnt work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mikebob 14:e66cf781f5b9 1 //Enhancement 2//
Mikebob 16:afe936377d66 2 //Enhancement 2//
Mikebob 16:afe936377d66 3
Mikebob 14:e66cf781f5b9 4 #include "mbed.h"
Mikebob 16:afe936377d66 5
Mikebob 14:e66cf781f5b9 6 //Status LED
Mikebob 14:e66cf781f5b9 7 DigitalOut led(LED1);
Mikebob 16:afe936377d66 8
Mikebob 14:e66cf781f5b9 9 //Motor PWM (speed)
Mikebob 14:e66cf781f5b9 10 PwmOut PWMA(PA_8);
Mikebob 14:e66cf781f5b9 11 PwmOut PWMB(PB_4);
Mikebob 16:afe936377d66 12
Mikebob 14:e66cf781f5b9 13 //Motor Direction
Mikebob 14:e66cf781f5b9 14 DigitalOut DIRA(PA_9);
Mikebob 14:e66cf781f5b9 15 DigitalOut DIRB(PB_10);
Mikebob 16:afe936377d66 16
Mikebob 14:e66cf781f5b9 17 //Hall-Effect Sensor Inputs
Mikebob 14:e66cf781f5b9 18 DigitalIn HEA1(PB_2);
Mikebob 14:e66cf781f5b9 19 DigitalIn HEA2(PB_1);
Mikebob 14:e66cf781f5b9 20 DigitalIn HEB1(PB_15);
Mikebob 14:e66cf781f5b9 21 DigitalIn HEB2(PB_14);
Mikebob 16:afe936377d66 22
Mikebob 14:e66cf781f5b9 23 //On board switch
Mikebob 14:e66cf781f5b9 24 DigitalIn SW1(USER_BUTTON);
Mikebob 16:afe936377d66 25
Mikebob 14:e66cf781f5b9 26 //Use the serial object so we can use higher speeds
Mikebob 14:e66cf781f5b9 27 Serial terminal(USBTX, USBRX);
Mikebob 16:afe936377d66 28
Mikebob 14:e66cf781f5b9 29 //Timer used for measuring speeds
Mikebob 14:e66cf781f5b9 30 Timer timer;
Mikebob 14:e66cf781f5b9 31 Timer timer1;
Mikebob 16:afe936377d66 32
Mikebob 14:e66cf781f5b9 33 //Enumerated types
Mikebob 14:e66cf781f5b9 34 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 14:e66cf781f5b9 35 enum PULSE {NOPULSE=0, PULSE};
Mikebob 14:e66cf781f5b9 36 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 14:e66cf781f5b9 37
Mikebob 14:e66cf781f5b9 38 //Duty cycles
Mikebob 14:e66cf781f5b9 39 float dutyA = 1.0f; //100%
Mikebob 14:e66cf781f5b9 40 float dutyB = 1.0f; //100%
Mikebob 14:e66cf781f5b9 41 //Array of sensor data
Mikebob 14:e66cf781f5b9 42 int tA1[2];
Mikebob 14:e66cf781f5b9 43 int tA2[2];
Mikebob 14:e66cf781f5b9 44 int tB1[2];
Mikebob 14:e66cf781f5b9 45 int tB2[2];
Mikebob 14:e66cf781f5b9 46 float dis;
Mikebob 14:e66cf781f5b9 47 float trav =0;
Mikebob 14:e66cf781f5b9 48 float speedA, speedB = 1.0f;
Mikebob 16:afe936377d66 49 float paceA = 1.0f;
Mikebob 16:afe936377d66 50 float paceB = 1.0f;
Mikebob 14:e66cf781f5b9 51 void time()
Mikebob 14:e66cf781f5b9 52 {
Mikebob 14:e66cf781f5b9 53 //Reset timer and Start
Mikebob 14:e66cf781f5b9 54 timer.reset();
Mikebob 14:e66cf781f5b9 55 timer.start();
Mikebob 14:e66cf781f5b9 56
Mikebob 14:e66cf781f5b9 57 //**********************
Mikebob 14:e66cf781f5b9 58 //TIME THE FULL SEQUENCE
Mikebob 14:e66cf781f5b9 59 //**********************
Mikebob 16:afe936377d66 60
Mikebob 14:e66cf781f5b9 61 //Wait for rising edge of A1 and log time
Mikebob 14:e66cf781f5b9 62 while (HEA1 == NOPULSE);
Mikebob 14:e66cf781f5b9 63 tA1[0] = timer.read_us();
Mikebob 14:e66cf781f5b9 64 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 14:e66cf781f5b9 65 while (HEA2 == NOPULSE);
Mikebob 14:e66cf781f5b9 66 tA2[0] = timer.read_us();
Mikebob 14:e66cf781f5b9 67 //Wait for falling edge of A1
Mikebob 14:e66cf781f5b9 68 while (HEA1 == PULSE);
Mikebob 14:e66cf781f5b9 69 tA1[1] = timer.read_us();
Mikebob 14:e66cf781f5b9 70 //Wait for falling edge of A2
Mikebob 14:e66cf781f5b9 71 while (HEA2 == PULSE);
Mikebob 14:e66cf781f5b9 72 tA2[1] = timer.read_us();
Mikebob 14:e66cf781f5b9 73 //Wait for rising edge of B1 and log time
Mikebob 14:e66cf781f5b9 74 while (HEB1 == NOPULSE);
Mikebob 14:e66cf781f5b9 75 tB1[0] = timer.read_us();
Mikebob 14:e66cf781f5b9 76 //Wait for rising edge of B2 and log time (30 degrees?)
Mikebob 14:e66cf781f5b9 77 while (HEB2 == NOPULSE);
Mikebob 14:e66cf781f5b9 78 tB2[0] = timer.read_us();
Mikebob 14:e66cf781f5b9 79 //Wait for falling edge of B1
Mikebob 14:e66cf781f5b9 80 while (HEB1 == PULSE);
Mikebob 14:e66cf781f5b9 81 tB1[1] = timer.read_us();
Mikebob 14:e66cf781f5b9 82 //Wait for falling edge of B2
Mikebob 14:e66cf781f5b9 83 while (HEB2 == PULSE);
Mikebob 14:e66cf781f5b9 84 tB2[1] = timer.read_us();
Mikebob 14:e66cf781f5b9 85
Mikebob 14:e66cf781f5b9 86 terminal.printf("tA1(0) = %d\n", tA1[0]);
Mikebob 14:e66cf781f5b9 87 terminal.printf("tA1(1) = %d\n", tA1[1]);
Mikebob 14:e66cf781f5b9 88 terminal.printf("tA2(0) = %d\n", tA2[0]);
Mikebob 14:e66cf781f5b9 89 terminal.printf("tA2(1) = %d\n", tA2[1]);
Mikebob 14:e66cf781f5b9 90
Mikebob 14:e66cf781f5b9 91 //Calculate the frequency of rotation for A
Mikebob 14:e66cf781f5b9 92 float TA1 = 2.0f * (tA1[1]-tA1[0]);
Mikebob 14:e66cf781f5b9 93 float TA2 = 2.0f * (tA2[1]-tA2[0]);
Mikebob 14:e66cf781f5b9 94 float TA = (TA1 + TA2) * 0.5f;
Mikebob 14:e66cf781f5b9 95
Mikebob 14:e66cf781f5b9 96 //Calculate the frequency of rotation for B
Mikebob 14:e66cf781f5b9 97 float TB1 = 2.0f * (tB1[1]-tB1[0]);
Mikebob 14:e66cf781f5b9 98 float TB2 = 2.0f * (tB2[1]-tB2[0]);
Mikebob 14:e66cf781f5b9 99 float TB = (TB1 + TB2) * 0.5f;
Mikebob 14:e66cf781f5b9 100
Mikebob 14:e66cf781f5b9 101 dis = timer1.read_us();
Mikebob 14:e66cf781f5b9 102 float mm = ((TA*3)*20.8)/175.9;
Mikebob 14:e66cf781f5b9 103 trav = dis/mm;
Mikebob 14:e66cf781f5b9 104 float fA = 1.0f/ (TA *(float)3.0E-6); // FOR A
Mikebob 14:e66cf781f5b9 105 float fB = 1.0f/ (TB *(float)3.0E-6); // FOR B
Mikebob 14:e66cf781f5b9 106 terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n", fA/20.8f, fB/20.8f, trav);
Mikebob 14:e66cf781f5b9 107 speedA = fA/20.8f;
Mikebob 14:e66cf781f5b9 108 speedB = fB/20.8f;
Mikebob 14:e66cf781f5b9 109 }
Mikebob 16:afe936377d66 110
Mikebob 14:e66cf781f5b9 111 void reset()
Mikebob 14:e66cf781f5b9 112 {
Mikebob 14:e66cf781f5b9 113 timer1.reset();
Mikebob 14:e66cf781f5b9 114 time();
Mikebob 14:e66cf781f5b9 115 }
Mikebob 15:b7ca53716b05 116
Mikebob 16:afe936377d66 117 void oneRPS()
Mikebob 15:b7ca53716b05 118 {
Mikebob 15:b7ca53716b05 119 time();
Mikebob 15:b7ca53716b05 120 if (speedA <= 1.0f)
Mikebob 15:b7ca53716b05 121 {
Mikebob 15:b7ca53716b05 122 paceA +=0.01f;
Mikebob 15:b7ca53716b05 123 wait_ms(25);
Mikebob 15:b7ca53716b05 124 }
Mikebob 16:afe936377d66 125 if (speedA >= 1.0f)
Mikebob 15:b7ca53716b05 126 {
Mikebob 15:b7ca53716b05 127 paceA -=0.01f;
Mikebob 15:b7ca53716b05 128 wait_ms(25);
Mikebob 15:b7ca53716b05 129 }
Mikebob 16:afe936377d66 130 if (speedB <= 1.0f)
Mikebob 15:b7ca53716b05 131 {
Mikebob 15:b7ca53716b05 132 paceB +=0.01f;
Mikebob 15:b7ca53716b05 133 wait_ms(25);
Mikebob 15:b7ca53716b05 134 }
Mikebob 16:afe936377d66 135 if (speedB >= 1.0f)
Mikebob 15:b7ca53716b05 136 {
Mikebob 15:b7ca53716b05 137 paceB -=0.01f;
Mikebob 15:b7ca53716b05 138 wait_ms(25);
Mikebob 15:b7ca53716b05 139 }
Mikebob 16:afe936377d66 140 PWMA.write(paceA);
Mikebob 16:afe936377d66 141 PWMB.write(paceA);
Mikebob 15:b7ca53716b05 142 }
Mikebob 16:afe936377d66 143
Mikebob 14:e66cf781f5b9 144 int main()
Mikebob 14:e66cf781f5b9 145 {
Mikebob 16:afe936377d66 146
Mikebob 14:e66cf781f5b9 147 //Configure the terminal to high speed
Mikebob 14:e66cf781f5b9 148 terminal.baud(115200);
Mikebob 16:afe936377d66 149
Mikebob 14:e66cf781f5b9 150 //Set initial motor direction
Mikebob 14:e66cf781f5b9 151 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 152 DIRB = FORWARD;
Mikebob 16:afe936377d66 153
Mikebob 14:e66cf781f5b9 154 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 155 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 156 PWMB.period_ms(10);
Mikebob 16:afe936377d66 157
Mikebob 14:e66cf781f5b9 158 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 159 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 160 PWMB.write(0.0f); //0% duty cycle
Mikebob 16:afe936377d66 161
Mikebob 14:e66cf781f5b9 162 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 163 terminal.puts("Press USER button to start");
Mikebob 14:e66cf781f5b9 164 led = 0;
Mikebob 14:e66cf781f5b9 165 while (SW1 == RELEASED);
Mikebob 14:e66cf781f5b9 166 led = 1;
Mikebob 14:e66cf781f5b9 167 wait(0.5);
Mikebob 14:e66cf781f5b9 168 //Set align wheels
Mikebob 14:e66cf781f5b9 169 PWMA.write(1.0f); //Set duty cycle (%)
Mikebob 14:e66cf781f5b9 170 PWMB.write(1.0f); //Set duty cycle (%)
Mikebob 14:e66cf781f5b9 171 //*********************************************************************
Mikebob 14:e66cf781f5b9 172 //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)
Mikebob 14:e66cf781f5b9 173 //*********************************************************************
Mikebob 16:afe936377d66 174
Mikebob 14:e66cf781f5b9 175 //Wait for rising edge of A1 and log time
Mikebob 14:e66cf781f5b9 176 while (HEA1 == NOPULSE);
Mikebob 14:e66cf781f5b9 177 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 14:e66cf781f5b9 178 while (HEA2 == NOPULSE);
Mikebob 14:e66cf781f5b9 179 //Wait for falling edge of A1
Mikebob 14:e66cf781f5b9 180 while (HEA1 == PULSE);
Mikebob 14:e66cf781f5b9 181 //Wait for falling edge of A2
Mikebob 14:e66cf781f5b9 182 while (HEA2 == PULSE);
Mikebob 14:e66cf781f5b9 183 //Wait for rising edge of B1 and log time
Mikebob 14:e66cf781f5b9 184 while (HEB1 == NOPULSE);
Mikebob 14:e66cf781f5b9 185 //Wait for rising edge of B2 and log time (30 degrees?)
Mikebob 14:e66cf781f5b9 186 while (HEB2 == NOPULSE);
Mikebob 14:e66cf781f5b9 187 //Wait for falling edge of B1
Mikebob 14:e66cf781f5b9 188 while (HEB1 == PULSE);
Mikebob 14:e66cf781f5b9 189 //Wait for falling edge of B2
Mikebob 14:e66cf781f5b9 190 while (HEB2 == PULSE);
Mikebob 16:afe936377d66 191
Mikebob 14:e66cf781f5b9 192 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 193 PWMA.write(0.0f); //Set duty cycle (%)
Mikebob 14:e66cf781f5b9 194 PWMB.write(0.0f); //Set duty cycle (%)
Mikebob 16:afe936377d66 195
Mikebob 14:e66cf781f5b9 196 //Wait - give time to start running
Mikebob 14:e66cf781f5b9 197 wait(1.0);
Mikebob 14:e66cf781f5b9 198 timer1.reset();
Mikebob 14:e66cf781f5b9 199 timer1.start();
Mikebob 14:e66cf781f5b9 200 //Main polling loop
Mikebob 16:afe936377d66 201
Mikebob 16:afe936377d66 202
Mikebob 14:e66cf781f5b9 203 PWMA.write(paceA); //Set duty cycle (%)
Mikebob 14:e66cf781f5b9 204 PWMB.write(paceB);
Mikebob 14:e66cf781f5b9 205 while(1)
Mikebob 14:e66cf781f5b9 206 {
Mikebob 16:afe936377d66 207
Mikebob 16:afe936377d66 208 while (trav <= 1250)
Mikebob 16:afe936377d66 209 {
Mikebob 16:afe936377d66 210 oneRPS();
Mikebob 16:afe936377d66 211 }
Mikebob 16:afe936377d66 212 PWMA.write(0.0f); //Set duty cycle (%)
Mikebob 16:afe936377d66 213 PWMB.write(0.0f);
Mikebob 16:afe936377d66 214 break;
Mikebob 14:e66cf781f5b9 215 }
Mikebob 14:e66cf781f5b9 216 }