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@1:2a6b6d2112ba, 2019-11-05 (annotated)

Committer:
Mikebob
Date:
Tue Nov 05 12:31:24 2019 +0000
Revision:
1:2a6b6d2112ba
Parent:
0:81f6a19a5a44
Child:
2:3ecc1de21269
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mikebob 0:81f6a19a5a44 1 /*
Mikebob 1:2a6b6d2112ba 2 Version 3 – Mike's edit
Mikebob 0:81f6a19a5a44 3 */
Mikebob 0:81f6a19a5a44 4
Mikebob 0:81f6a19a5a44 5 #include "mbed.h"
Mikebob 0:81f6a19a5a44 6 //Status LED
Mikebob 0:81f6a19a5a44 7 DigitalOut led(LED1);
Mikebob 0:81f6a19a5a44 8 //Motor PWM (speed)
Mikebob 0:81f6a19a5a44 9 PwmOut PWMA(PA_8);
Mikebob 0:81f6a19a5a44 10 PwmOut PWMB(PB_4);
Mikebob 0:81f6a19a5a44 11 //Motor Direction
Mikebob 0:81f6a19a5a44 12 DigitalOut DIRA(PA_9);
Mikebob 0:81f6a19a5a44 13 DigitalOut DIRB(PB_10);
Mikebob 0:81f6a19a5a44 14 //Hall-Effect Sensor Inputs
Mikebob 0:81f6a19a5a44 15 DigitalIn HEA1(PB_2);
Mikebob 0:81f6a19a5a44 16 DigitalIn HEA2(PB_1);
Mikebob 0:81f6a19a5a44 17 DigitalIn HEB1(PB_15);
Mikebob 0:81f6a19a5a44 18 DigitalIn HEB2(PB_14);
Mikebob 0:81f6a19a5a44 19 //On board switch
Mikebob 0:81f6a19a5a44 20 DigitalIn SW1(USER_BUTTON);
Mikebob 0:81f6a19a5a44 21 //Use the serial object so we can use higher speeds
Mikebob 0:81f6a19a5a44 22 Serial terminal(USBTX, USBRX);
Mikebob 0:81f6a19a5a44 23 //Timer used for measuring speeds
Mikebob 0:81f6a19a5a44 24 Timer timer;
Mikebob 0:81f6a19a5a44 25 //Enumerated types
Mikebob 0:81f6a19a5a44 26 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 0:81f6a19a5a44 27 enum PULSE {NOPULSE=0, PULSE};
Mikebob 0:81f6a19a5a44 28 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 0:81f6a19a5a44 29 //Debug GPIO
Mikebob 0:81f6a19a5a44 30 DigitalOut probe(D10);
Mikebob 0:81f6a19a5a44 31 //Duty cycles
Mikebob 0:81f6a19a5a44 32 float dutyA = 1.0f; //100%
Mikebob 0:81f6a19a5a44 33 float dutyB = 0.99f; //100%
Mikebob 0:81f6a19a5a44 34 int main()
Mikebob 0:81f6a19a5a44 35 {
Mikebob 0:81f6a19a5a44 36 //Configure the terminal to high speed
Mikebob 0:81f6a19a5a44 37 terminal.baud(115200);
Mikebob 0:81f6a19a5a44 38 //Set initial motor direction
Mikebob 0:81f6a19a5a44 39 DIRA = FORWARD;
Mikebob 0:81f6a19a5a44 40 DIRB = FORWARD;
Mikebob 0:81f6a19a5a44 41 //Set motor period to 100Hz
Mikebob 0:81f6a19a5a44 42 PWMA.period_ms(10);
Mikebob 0:81f6a19a5a44 43 PWMB.period_ms(10);
Mikebob 0:81f6a19a5a44 44 //Set initial motor speed to stop
Mikebob 0:81f6a19a5a44 45 PWMA.write(0.0f); //0% duty cycle
Mikebob 0:81f6a19a5a44 46 PWMB.write(0.0f); //0% duty cycle
Mikebob 0:81f6a19a5a44 47 //Wait for USER button (blue pull-down switch) to start
Mikebob 0:81f6a19a5a44 48 terminal.puts("Press USER button to start");
Mikebob 0:81f6a19a5a44 49 led = 0;
Mikebob 0:81f6a19a5a44 50 while (SW1 == RELEASED);
Mikebob 0:81f6a19a5a44 51 led = 1;
Mikebob 0:81f6a19a5a44 52 //Set initial motor speed to stop {
Mikebob 0:81f6a19a5a44 53 for(float ramp = 0.0f; ramp <= 1.0f ; ramp += 0.1)
Mikebob 0:81f6a19a5a44 54 {
Mikebob 0:81f6a19a5a44 55 PWMA.write(ramp); //Set duty cycle y
Mikebob 0:81f6a19a5a44 56 PWMB.write(ramp-0.011); //Set duty cycle y
Mikebob 0:81f6a19a5a44 57 wait(1);
Mikebob 0:81f6a19a5a44 58 }
Mikebob 0:81f6a19a5a44 59 PWMA.write(dutyA); //Set duty cycle hyp
Mikebob 0:81f6a19a5a44 60 PWMB.write(dutyB); //Set duty cycle hyp
Mikebob 0:81f6a19a5a44 61 wait(0.6);
Mikebob 0:81f6a19a5a44 62 PWMB.write(0.2f); //turn 31deg
Mikebob 0:81f6a19a5a44 63 wait(1.6);
Mikebob 0:81f6a19a5a44 64 PWMA.write(dutyA); //Set duty cycle hyp
Mikebob 0:81f6a19a5a44 65 PWMB.write(dutyB); //Set duty cycle hyp
Mikebob 0:81f6a19a5a44 66 wait(4.4);
Mikebob 0:81f6a19a5a44 67 PWMB.write(0.2f); //turn 59deg
Mikebob 0:81f6a19a5a44 68 wait(1.1);
Mikebob 0:81f6a19a5a44 69 PWMA.write(dutyA); //Set duty cycle x
Mikebob 0:81f6a19a5a44 70 PWMB.write(dutyB); //Set duty cycle x
Mikebob 0:81f6a19a5a44 71 wait(2.3);
Mikebob 0:81f6a19a5a44 72 PWMB.write(0.2f); //turn 45deg
Mikebob 0:81f6a19a5a44 73 wait(0.7);
Mikebob 0:81f6a19a5a44 74 //Array of sensor data
Mikebob 0:81f6a19a5a44 75 int tA1[2];
Mikebob 0:81f6a19a5a44 76 int tA2[2];
Mikebob 0:81f6a19a5a44 77 PWMA.write(0.0f);
Mikebob 0:81f6a19a5a44 78 PWMB.write(0.0f);
Mikebob 0:81f6a19a5a44 79 //Instructions to user
Mikebob 0:81f6a19a5a44 80 terminal.puts("Press USER button repeatedly to adapt duty (to convernge on 1 rotation/s)");
Mikebob 0:81f6a19a5a44 81 //Main polling loop
Mikebob 0:81f6a19a5a44 82 while(1) {
Mikebob 0:81f6a19a5a44 83 //Reset timer and Start
Mikebob 0:81f6a19a5a44 84 timer.reset();
Mikebob 0:81f6a19a5a44 85 timer.start();
Mikebob 0:81f6a19a5a44 86
Mikebob 0:81f6a19a5a44 87 //*********************************************************************
Mikebob 0:81f6a19a5a44 88
Mikebob 0:81f6a19a5a44 89 //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)
Mikebob 0:81f6a19a5a44 90
Mikebob 0:81f6a19a5a44 91 //*********************************************************************
Mikebob 0:81f6a19a5a44 92
Mikebob 0:81f6a19a5a44 93 //Wait for rising edge of A1 and log time
Mikebob 0:81f6a19a5a44 94 while (HEA1 == NOPULSE);
Mikebob 0:81f6a19a5a44 95 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 0:81f6a19a5a44 96 while (HEA2 == NOPULSE);
Mikebob 0:81f6a19a5a44 97 //Wait for falling edge of A1
Mikebob 0:81f6a19a5a44 98 while (HEA1 == PULSE);
Mikebob 0:81f6a19a5a44 99 //Wait for falling edge of A2
Mikebob 0:81f6a19a5a44 100 while (HEA2 == PULSE);
Mikebob 0:81f6a19a5a44 101 //***********************
Mikebob 0:81f6a19a5a44 102 //TIME THE FULL SEQUENCE
Mikebob 0:81f6a19a5a44 103 //**********************
Mikebob 0:81f6a19a5a44 104 //Wait for rising edge of A1 and log time
Mikebob 0:81f6a19a5a44 105 while (HEA1 == NOPULSE);
Mikebob 0:81f6a19a5a44 106 tA1[0] = timer.read_us();
Mikebob 0:81f6a19a5a44 107 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 0:81f6a19a5a44 108 while (HEA2 == NOPULSE);
Mikebob 0:81f6a19a5a44 109 tA2[0] = timer.read_us();
Mikebob 0:81f6a19a5a44 110 //Wait for falling edge of A1
Mikebob 0:81f6a19a5a44 111 while (HEA1 == PULSE);
Mikebob 0:81f6a19a5a44 112 tA1[1] = timer.read_us();
Mikebob 0:81f6a19a5a44 113 //Wait for falling edge of A2
Mikebob 0:81f6a19a5a44 114 while (HEA2 == PULSE);
Mikebob 0:81f6a19a5a44 115 tA2[1] = timer.read_us();
Mikebob 0:81f6a19a5a44 116 terminal.printf("tA1(0) = %d\n", tA1[0]);
Mikebob 0:81f6a19a5a44 117 terminal.printf("tA1(1) = %d\n", tA1[1]);
Mikebob 0:81f6a19a5a44 118 terminal.printf("tA2(0) = %d\n", tA2[0]);
Mikebob 0:81f6a19a5a44 119 terminal.printf("tA2(1) = %d\n", tA2[1]);
Mikebob 0:81f6a19a5a44 120 //Calculate the frequency of rotation
Mikebob 0:81f6a19a5a44 121 float TA1 = 2.0f * (tA1[1]-tA1[0]);
Mikebob 0:81f6a19a5a44 122 float TA2 = 2.0f * (tA2[1]-tA2[0]);
Mikebob 0:81f6a19a5a44 123 float TA = (TA1 + TA2) * 0.5f;
Mikebob 0:81f6a19a5a44 124 float fA = 1.0f/ (TA *(float)3.0E-6);
Mikebob 0:81f6a19a5a44 125 terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA, fA/20.2f);
Mikebob 0:81f6a19a5a44 126 //Reset timers
Mikebob 0:81f6a19a5a44 127 timer.stop();
Mikebob 0:81f6a19a5a44 128 //Wait for button press
Mikebob 0:81f6a19a5a44 129 while (SW1 == 1);
Mikebob 0:81f6a19a5a44 130 wait(0.2);
Mikebob 0:81f6a19a5a44 131 while (SW1 == 0);
Mikebob 0:81f6a19a5a44 132 wait(0.1);
Mikebob 0:81f6a19a5a44 133 //******************************************
Mikebob 0:81f6a19a5a44 134
Mikebob 0:81f6a19a5a44 135 //Adapt duty to meet 1 revolution per second
Mikebob 0:81f6a19a5a44 136
Mikebob 0:81f6a19a5a44 137 //******************************************
Mikebob 0:81f6a19a5a44 138 float wA = fA/20.2f; //Wheel speed
Mikebob 0:81f6a19a5a44 139 float deltaA = 1.0f-wA; //Error
Mikebob 0:81f6a19a5a44 140 dutyA = dutyA + deltaA*0.1f; //Increase duty in proportion to the error
Mikebob 0:81f6a19a5a44 141 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 0:81f6a19a5a44 142 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 0:81f6a19a5a44 143 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 0:81f6a19a5a44 144 //Update duty cycle to correct in the first direction
Mikebob 0:81f6a19a5a44 145 PWMA.write(dutyA);
Mikebob 0:81f6a19a5a44 146 //Echo to the terminal
Mikebob 0:81f6a19a5a44 147 terminal.printf("Adapting duty cycle to %6.2f\n", dutyA);
Mikebob 0:81f6a19a5a44 148 }
Mikebob 0:81f6a19a5a44 149 }
Mikebob 0:81f6a19a5a44 150
Mikebob 0:81f6a19a5a44 151
Mikebob 0:81f6a19a5a44 152
Mikebob 0:81f6a19a5a44 153
Mikebob 0:81f6a19a5a44 154
Mikebob 0:81f6a19a5a44 155
Mikebob 0:81f6a19a5a44 156
Mikebob 0:81f6a19a5a44 157
Mikebob 0:81f6a19a5a44 158
Mikebob 0:81f6a19a5a44 159
Mikebob 0:81f6a19a5a44 160
Mikebob 0:81f6a19a5a44 161