Angus McDowall / Mbed 2 deprecated ROCO104_Ultrasound_SPC

pabs

Dependencies:   mbed motor

main.cpp@5:2621fc2ed6da, 2019-05-06 (annotated)

Committer:
awmcdowall
Date:
Mon May 06 10:25:13 2019 +0000
Revision:
5:2621fc2ed6da
Parent:
4:8249fab4d8d3 
pabs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
martinsimpson 0:51c12cc34baf 1 /*
martinsimpson 1:3ca91ad8e927 2 Simple Routine for Nucleo Board for ROCO104 Buggy Motor Control and Microswitches
martinsimpson 1:3ca91ad8e927 3 Heavy edit from previous ROCO103PP code
martinsimpson 1:3ca91ad8e927 4 Motor Class can now be instansiated with all four pins needed to control the H Bridge
martinsimpson 1:3ca91ad8e927 5 with a member functions as follows
martinsimpson 1:3ca91ad8e927 6
martinsimpson 1:3ca91ad8e927 7 Motor::Speed(float A, Float B) range -0.1 to +1.0 to give full reverse to full forward for A/B motors
martinsimpson 1:3ca91ad8e927 8 Motor::Stop() STOP
martinsimpson 1:3ca91ad8e927 9 Motor::Fwd(float) Forward but floating point number (range 0.0 to 1.0)
martinsimpson 1:3ca91ad8e927 10 Motor::Rev(float) Reverse but floating point number (range 0.0 to 1.0)
martinsimpson 1:3ca91ad8e927 11
martinsimpson 0:51c12cc34baf 12 Plymouth University
martinsimpson 0:51c12cc34baf 13 M.Simpson 31st October 2016
martinsimpson 1:3ca91ad8e927 14 Edited 03/02/2017
martinsimpson 1:3ca91ad8e927 15 Edited 06/12/2018
martinsimpson 0:51c12cc34baf 16 */
martinsimpson 0:51c12cc34baf 17 #include "mbed.h"
martinsimpson 0:51c12cc34baf 18 #include "motor.h"
awmcdowall 5:2621fc2ed6da 19 #include "UltraSonic.h"
awmcdowall 5:2621fc2ed6da 20 #include "spc.h"
martinsimpson 1:3ca91ad8e927 21
martinsimpson 0:51c12cc34baf 22 #define TIME_PERIOD 2 //Constant compiler Values here 2 equates to 2ms or 500Hz base Frequency
martinsimpson 0:51c12cc34baf 23 #define DUTY 0.9 //DUTY of 1.0=100%, 0.4=40% etc.,
martinsimpson 0:51c12cc34baf 24
martinsimpson 0:51c12cc34baf 25 DigitalIn microswitch1(D4); //Instance of the DigitalIn class called 'microswitch1'
martinsimpson 0:51c12cc34baf 26 DigitalIn microswitch2(D3); //Instance of the DigitalIn class called 'microswitch2'
martinsimpson 0:51c12cc34baf 27
martinsimpson 1:3ca91ad8e927 28 Motor Wheel(D13,D11,D9,D10); //Instance of the Motor Class called 'Wheel' see motor.h and motor.cpp
martinsimpson 0:51c12cc34baf 29
awmcdowall 5:2621fc2ed6da 30
martinsimpson 0:51c12cc34baf 31 DigitalIn myButton(USER_BUTTON); //USER_BUTTON is the Blue Button on the NUCLEO Board
martinsimpson 0:51c12cc34baf 32
martinsimpson 1:3ca91ad8e927 33 DigitalOut led(LED3); //LED1 is the Green LED on the NUCLEO board
martinsimpson 0:51c12cc34baf 34 //N.B. The RED LED is the POWER Indicator
martinsimpson 0:51c12cc34baf 35 //and the Multicoloured LED indicates status of the ST-LINK Programming cycle
martinsimpson 0:51c12cc34baf 36
awmcdowall 5:2621fc2ed6da 37 Serial terminal(USBTX,USBRX); //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
martinsimpson 0:51c12cc34baf 38 //This is Pseudo RS232 over USB the NUCLEO will appear as a COMx Port see device Manager on PC used
martinsimpson 0:51c12cc34baf 39 //Use PuTTY to monitor check COMx and BAUD rate (115200)
awmcdowall 5:2621fc2ed6da 40
awmcdowall 5:2621fc2ed6da 41 DigitalOut red(D6); //Three coloured LEDs for the single Pixel Camera
awmcdowall 5:2621fc2ed6da 42 DigitalOut green(D12);
awmcdowall 5:2621fc2ed6da 43 DigitalOut blue(D8);
awmcdowall 5:2621fc2ed6da 44
awmcdowall 5:2621fc2ed6da 45 AnalogIn ldr(A1); //Instance of the AnalogIn class called 'ldr'
awmcdowall 5:2621fc2ed6da 46
awmcdowall 5:2621fc2ed6da 47 void SPCoff(void){red=0;green=0;blue=0;}
awmcdowall 5:2621fc2ed6da 48
awmcdowall 5:2621fc2ed6da 49 void SPCtest(void)
awmcdowall 5:2621fc2ed6da 50 {
awmcdowall 5:2621fc2ed6da 51 float rlight,glight,blight;
awmcdowall 5:2621fc2ed6da 52
awmcdowall 5:2621fc2ed6da 53 red=1;green=0;blue=0; //ILLUMINATE WITH RED ONLY!
awmcdowall 5:2621fc2ed6da 54 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 55 rlight=ldr/RCOR; //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
awmcdowall 5:2621fc2ed6da 56 printf("RED %4.2fV ",rlight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 57 wait(0.1f);
awmcdowall 5:2621fc2ed6da 58
awmcdowall 5:2621fc2ed6da 59 red=0;green=1;blue=0; //ILLUMINATE WITH GREEN ONLY!
awmcdowall 5:2621fc2ed6da 60 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 61 glight=ldr/GCOR; //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
awmcdowall 5:2621fc2ed6da 62 printf("GRN %4.2fV ",glight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 63 wait(0.1f);
awmcdowall 5:2621fc2ed6da 64
awmcdowall 5:2621fc2ed6da 65 red=0;green=0;blue=1; //ILLUMINATE WITH BLUE ONLY!
awmcdowall 5:2621fc2ed6da 66 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 67 blight=ldr/BCOR; //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
awmcdowall 5:2621fc2ed6da 68 printf("BLU %4.2fV\n\r",blight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 69 wait(0.1f);
awmcdowall 5:2621fc2ed6da 70
awmcdowall 5:2621fc2ed6da 71 red=0;green=0;blue=0;
awmcdowall 5:2621fc2ed6da 72 if(rlight<glight && rlight<blight){printf("\n\rRED!\n\n\r"); SPCflash(RED,5);}
awmcdowall 5:2621fc2ed6da 73 if(glight<rlight && glight<blight){printf("\n\rGREEN!\n\n\r");SPCflash(GRN,5);}
awmcdowall 5:2621fc2ed6da 74 if(blight<glight && blight<rlight){printf("\n\rBLUE!\n\n\r"); SPCflash(BLU,5);}
awmcdowall 5:2621fc2ed6da 75 wait(1.0f);
awmcdowall 5:2621fc2ed6da 76 }
awmcdowall 5:2621fc2ed6da 77
awmcdowall 5:2621fc2ed6da 78 void SPCflash(int colour,int numberOfFlashes)
awmcdowall 5:2621fc2ed6da 79 {
awmcdowall 5:2621fc2ed6da 80 switch (colour)
awmcdowall 5:2621fc2ed6da 81 {
awmcdowall 5:2621fc2ed6da 82 case RED:for(int i=0;i<numberOfFlashes*2;i++){red=!red;wait(0.15f);}break;
awmcdowall 5:2621fc2ed6da 83 case GRN:for(int i=0;i<numberOfFlashes*2;i++){green=!green;wait(0.15f);}break;
awmcdowall 5:2621fc2ed6da 84 case BLU:for(int i=0;i<numberOfFlashes*2;i++){blue=!blue;wait(0.15f);}break;
awmcdowall 5:2621fc2ed6da 85 default:break;
awmcdowall 5:2621fc2ed6da 86 }
awmcdowall 5:2621fc2ed6da 87 }
awmcdowall 5:2621fc2ed6da 88
awmcdowall 5:2621fc2ed6da 89
awmcdowall 5:2621fc2ed6da 90 DigitalOut Trigger(D7); //Instance of the DigitalInOut class called 'TriggerEcho' WAS D6
awmcdowall 5:2621fc2ed6da 91 DigitalIn Echo(D2); //Instance of the DigitalInOut class called 'TriggerEcho'
awmcdowall 5:2621fc2ed6da 92 Timer pulse; //Instance of the Timer class called 'pulse' so we can measure timed events
awmcdowall 5:2621fc2ed6da 93
awmcdowall 5:2621fc2ed6da 94
awmcdowall 5:2621fc2ed6da 95 // Function ultra_sonic_distance() will load the global variable distance with Ultra Sonic Sensor value in mm
awmcdowall 5:2621fc2ed6da 96 // and then send the value to the stdio ouput i.e serial over USB
awmcdowall 5:2621fc2ed6da 97 void ultra_sonic_distance(void)
awmcdowall 5:2621fc2ed6da 98 {
awmcdowall 5:2621fc2ed6da 99 printf("%dmm \n\r",(int)GetDistance());
awmcdowall 5:2621fc2ed6da 100 }
awmcdowall 5:2621fc2ed6da 101
awmcdowall 5:2621fc2ed6da 102 // Function GetDistance() will return a float value of the Distance from the Ultrasonic Sensor in millimetres typically use as: ‘(float)myDistance=GetDistance();’
awmcdowall 5:2621fc2ed6da 103 float GetDistance()
awmcdowall 5:2621fc2ed6da 104 { //Function Name to be called
awmcdowall 5:2621fc2ed6da 105 int EchoPulseWidth=0,EchoStart=0,EchoEnd=0; //Assign and set to zero the local variables for this function
awmcdowall 5:2621fc2ed6da 106 Trigger = 1; //Signal goes High i.e. 3V3
awmcdowall 5:2621fc2ed6da 107 wait_us(100); //Wait 100us to give a pulse width Triggering the Ultrasonic Module
awmcdowall 5:2621fc2ed6da 108 Trigger = 0; //Signal goes Low i.e. 0V
awmcdowall 5:2621fc2ed6da 109 pulse.start(); //Start the instance of the class timer
awmcdowall 5:2621fc2ed6da 110 pulse.reset(); //Reset the instance of the Timer Class
awmcdowall 5:2621fc2ed6da 111 while(Echo == 0 && EchoStart < 25000){ //wait for Echo to go high
awmcdowall 5:2621fc2ed6da 112 EchoStart=pulse.read_us(); //Conditional 'AND' with timeout to prevent blocking!
awmcdowall 5:2621fc2ed6da 113 }
awmcdowall 5:2621fc2ed6da 114 while(Echo == 1 && ((EchoEnd - EchoStart) < 25000)){//wait for echo to return low
awmcdowall 5:2621fc2ed6da 115 EchoEnd=pulse.read_us(); //Conditional 'OR' with timeout to prevent blocking!
awmcdowall 5:2621fc2ed6da 116 }
awmcdowall 5:2621fc2ed6da 117 EchoPulseWidth = EchoEnd - EchoStart; //time period in us
awmcdowall 5:2621fc2ed6da 118 pulse.stop(); //Stop the instance of the class timer
awmcdowall 5:2621fc2ed6da 119 return (float)EchoPulseWidth/5.8f; //calculate distance in mm and return the value as a float
awmcdowall 5:2621fc2ed6da 120 }
martinsimpson 0:51c12cc34baf 121
martinsimpson 0:51c12cc34baf 122
martinsimpson 0:51c12cc34baf 123 //Variable 'duty' for programmer to use to vary speed as required set here to #define compiler constant see above
martinsimpson 0:51c12cc34baf 124 float duty=DUTY;
martinsimpson 0:51c12cc34baf 125 //
awmcdowall 5:2621fc2ed6da 126 int main(){
awmcdowall 5:2621fc2ed6da 127 terminal.baud(115200);
awmcdowall 5:2621fc2ed6da 128 while(1){
awmcdowall 5:2621fc2ed6da 129 SPCoff();
awmcdowall 5:2621fc2ed6da 130 SPCtest();
awmcdowall 5:2621fc2ed6da 131 wait(0.5);
awmcdowall 5:2621fc2ed6da 132 }
awmcdowall 5:2621fc2ed6da 133 }
martinsimpson 4:8249fab4d8d3 134
martinsimpson 4:8249fab4d8d3 135
martinsimpson 1:3ca91ad8e927 136
martinsimpson 4:8249fab4d8d3 137 /*
martinsimpson 4:8249fab4d8d3 138 //Consider these lines of code to Accelerate the motors
martinsimpson 4:8249fab4d8d3 139 for (float i=0.5f; i<=1.0f; i+=0.01f) //Accelerate from 50% to 100%
martinsimpson 4:8249fab4d8d3 140 {
martinsimpson 4:8249fab4d8d3 141 Wheel.Speed(i,i);
martinsimpson 4:8249fab4d8d3 142 wait(0.1f);
martinsimpson 4:8249fab4d8d3 143 }
martinsimpson 4:8249fab4d8d3 144 */