Pablo Pelaez / Mbed 2 deprecated ROCO104_Ultrasound_SPC_LOOP

ROCO104

Dependencies:   mbed motor

main.cpp@6:dfc31e7e2c23, 2019-05-07 (annotated)

Committer:
Pabs44
Date:
Tue May 07 14:47:34 2019 +0000
Revision:
6:dfc31e7e2c23
Parent:
5:2621fc2ed6da 
code

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 1:3ca91ad8e927 25 Motor Wheel(D13,D11,D9,D10); //Instance of the Motor Class called 'Wheel' see motor.h and motor.cpp
martinsimpson 0:51c12cc34baf 26
martinsimpson 0:51c12cc34baf 27 DigitalIn myButton(USER_BUTTON); //USER_BUTTON is the Blue Button on the NUCLEO Board
martinsimpson 0:51c12cc34baf 28
martinsimpson 1:3ca91ad8e927 29 DigitalOut led(LED3); //LED1 is the Green LED on the NUCLEO board
martinsimpson 0:51c12cc34baf 30 //N.B. The RED LED is the POWER Indicator
Pabs44 6:dfc31e7e2c23 31 //and the Multicolored LED indicates status of the ST-LINK Programming cycle
martinsimpson 0:51c12cc34baf 32
Pabs44 6:dfc31e7e2c23 33 Serial pc(USBTX,USBRX); //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
martinsimpson 0:51c12cc34baf 34 //This is Pseudo RS232 over USB the NUCLEO will appear as a COMx Port see device Manager on PC used
martinsimpson 0:51c12cc34baf 35 //Use PuTTY to monitor check COMx and BAUD rate (115200)
awmcdowall 5:2621fc2ed6da 36
Pabs44 6:dfc31e7e2c23 37 DigitalOut red(D6); //Three colored LEDs for the single Pixel Camera
awmcdowall 5:2621fc2ed6da 38 DigitalOut green(D12);
awmcdowall 5:2621fc2ed6da 39 DigitalOut blue(D8);
awmcdowall 5:2621fc2ed6da 40
awmcdowall 5:2621fc2ed6da 41 AnalogIn ldr(A1); //Instance of the AnalogIn class called 'ldr'
awmcdowall 5:2621fc2ed6da 42
Pabs44 6:dfc31e7e2c23 43 DigitalOut Trigger(D7); //Instance of the DigitalInOut class called 'TriggerEcho' WAS D6
Pabs44 6:dfc31e7e2c23 44 DigitalIn Echo(D2); //Instance of the DigitalInOut class called 'TriggerEcho'
Pabs44 6:dfc31e7e2c23 45 Timer pulse; //Instance of the Timer class called 'pulse' so we can measure timed events
awmcdowall 5:2621fc2ed6da 46
Pabs44 6:dfc31e7e2c23 47 void SPCoff(void){ //Function to turn off SPC
Pabs44 6:dfc31e7e2c23 48 red=0;
Pabs44 6:dfc31e7e2c23 49 green=0;
Pabs44 6:dfc31e7e2c23 50 blue=0;
Pabs44 6:dfc31e7e2c23 51 }
Pabs44 6:dfc31e7e2c23 52
Pabs44 6:dfc31e7e2c23 53 void SPCtest(void){
awmcdowall 5:2621fc2ed6da 54 float rlight,glight,blight;
awmcdowall 5:2621fc2ed6da 55
Pabs44 6:dfc31e7e2c23 56 red=1; //ILLUMINATE WITH RED ONLY!
Pabs44 6:dfc31e7e2c23 57 green=0;
Pabs44 6:dfc31e7e2c23 58 blue=0;
awmcdowall 5:2621fc2ed6da 59 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 60 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 61 printf("RED %4.2fV ",rlight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 62 wait(0.1f);
awmcdowall 5:2621fc2ed6da 63
Pabs44 6:dfc31e7e2c23 64 red=0;
Pabs44 6:dfc31e7e2c23 65 green=1; //ILLUMINATE WITH GREEN ONLY!
Pabs44 6:dfc31e7e2c23 66 blue=0;
awmcdowall 5:2621fc2ed6da 67 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 68 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 69 printf("GRN %4.2fV ",glight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 70 wait(0.1f);
awmcdowall 5:2621fc2ed6da 71
Pabs44 6:dfc31e7e2c23 72 red=0;
Pabs44 6:dfc31e7e2c23 73 green=0;
Pabs44 6:dfc31e7e2c23 74 blue=1; //ILLUMINATE WITH BLUE ONLY!
awmcdowall 5:2621fc2ed6da 75 wait_ms(50); //Wait for LDR to respond leave this
awmcdowall 5:2621fc2ed6da 76 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 77 printf("BLU %4.2fV\n\r",blight); //Send LDR response in Volts to PC via PuTTY
awmcdowall 5:2621fc2ed6da 78 wait(0.1f);
awmcdowall 5:2621fc2ed6da 79
Pabs44 6:dfc31e7e2c23 80 red=0;
Pabs44 6:dfc31e7e2c23 81 green=0;
Pabs44 6:dfc31e7e2c23 82 blue=0;
awmcdowall 5:2621fc2ed6da 83 if(rlight<glight && rlight<blight){printf("\n\rRED!\n\n\r"); SPCflash(RED,5);}
awmcdowall 5:2621fc2ed6da 84 if(glight<rlight && glight<blight){printf("\n\rGREEN!\n\n\r");SPCflash(GRN,5);}
awmcdowall 5:2621fc2ed6da 85 if(blight<glight && blight<rlight){printf("\n\rBLUE!\n\n\r"); SPCflash(BLU,5);}
awmcdowall 5:2621fc2ed6da 86 wait(1.0f);
awmcdowall 5:2621fc2ed6da 87 }
awmcdowall 5:2621fc2ed6da 88
Pabs44 6:dfc31e7e2c23 89 void SPCflash(int color,int numberOfFlashes){ //Flash color in case of finding each respective one
Pabs44 6:dfc31e7e2c23 90 switch (color){
Pabs44 6:dfc31e7e2c23 91 case RED:for(int i=0;i<numberOfFlashes*2;i++){
Pabs44 6:dfc31e7e2c23 92 red=!red;
Pabs44 6:dfc31e7e2c23 93 wait(0.15f);
Pabs44 6:dfc31e7e2c23 94 }break;
Pabs44 6:dfc31e7e2c23 95 case GRN:for(int i=0;i<numberOfFlashes*2;i++){
Pabs44 6:dfc31e7e2c23 96 green=!green;
Pabs44 6:dfc31e7e2c23 97 wait(0.15f);
Pabs44 6:dfc31e7e2c23 98 }break;
Pabs44 6:dfc31e7e2c23 99 case BLU:for(int i=0;i<numberOfFlashes*2;i++){
Pabs44 6:dfc31e7e2c23 100 blue=!blue;
Pabs44 6:dfc31e7e2c23 101 wait(0.15f);
Pabs44 6:dfc31e7e2c23 102 }break;
awmcdowall 5:2621fc2ed6da 103 default:break;
awmcdowall 5:2621fc2ed6da 104 }
awmcdowall 5:2621fc2ed6da 105 }
awmcdowall 5:2621fc2ed6da 106
Pabs44 6:dfc31e7e2c23 107 // Function GetDistance() will return a float value of the Distance from the Ultrasonic Sensor in millimeters typically use as: ‘(float)myDistance=GetDistance();’
Pabs44 6:dfc31e7e2c23 108 float GetDistance(){ //Function Name to be called
awmcdowall 5:2621fc2ed6da 109 int EchoPulseWidth=0,EchoStart=0,EchoEnd=0; //Assign and set to zero the local variables for this function
awmcdowall 5:2621fc2ed6da 110 Trigger = 1; //Signal goes High i.e. 3V3
awmcdowall 5:2621fc2ed6da 111 wait_us(100); //Wait 100us to give a pulse width Triggering the Ultrasonic Module
awmcdowall 5:2621fc2ed6da 112 Trigger = 0; //Signal goes Low i.e. 0V
awmcdowall 5:2621fc2ed6da 113 pulse.start(); //Start the instance of the class timer
awmcdowall 5:2621fc2ed6da 114 pulse.reset(); //Reset the instance of the Timer Class
awmcdowall 5:2621fc2ed6da 115 while(Echo == 0 && EchoStart < 25000){ //wait for Echo to go high
awmcdowall 5:2621fc2ed6da 116 EchoStart=pulse.read_us(); //Conditional 'AND' with timeout to prevent blocking!
awmcdowall 5:2621fc2ed6da 117 }
awmcdowall 5:2621fc2ed6da 118 while(Echo == 1 && ((EchoEnd - EchoStart) < 25000)){//wait for echo to return low
awmcdowall 5:2621fc2ed6da 119 EchoEnd=pulse.read_us(); //Conditional 'OR' with timeout to prevent blocking!
awmcdowall 5:2621fc2ed6da 120 }
awmcdowall 5:2621fc2ed6da 121 EchoPulseWidth = EchoEnd - EchoStart; //time period in us
awmcdowall 5:2621fc2ed6da 122 pulse.stop(); //Stop the instance of the class timer
awmcdowall 5:2621fc2ed6da 123 return (float)EchoPulseWidth/5.8f; //calculate distance in mm and return the value as a float
awmcdowall 5:2621fc2ed6da 124 }
martinsimpson 0:51c12cc34baf 125
Pabs44 6:dfc31e7e2c23 126 // Function ultra_sonic_distance() will load the global variable distance with Ultra Sonic Sensor value in mm
Pabs44 6:dfc31e7e2c23 127 // and then send the value to the stdio ouput i.e serial over USB
Pabs44 6:dfc31e7e2c23 128 void ultra_sonic_distance(void){
Pabs44 6:dfc31e7e2c23 129 printf("%dmm \n\r",(int)GetDistance());
Pabs44 6:dfc31e7e2c23 130 }
martinsimpson 0:51c12cc34baf 131
martinsimpson 0:51c12cc34baf 132 //Variable 'duty' for programmer to use to vary speed as required set here to #define compiler constant see above
martinsimpson 0:51c12cc34baf 133 float duty=DUTY;
Pabs44 6:dfc31e7e2c23 134
awmcdowall 5:2621fc2ed6da 135 int main(){
Pabs44 6:dfc31e7e2c23 136 pc.baud(115200);
Pabs44 6:dfc31e7e2c23 137
Pabs44 6:dfc31e7e2c23 138 while(myButton==0){
Pabs44 6:dfc31e7e2c23 139 led=0;
Pabs44 6:dfc31e7e2c23 140 wait(0.1);
Pabs44 6:dfc31e7e2c23 141 led=1;
Pabs44 6:dfc31e7e2c23 142 wait(0.1);
Pabs44 6:dfc31e7e2c23 143 }
Pabs44 6:dfc31e7e2c23 144 printf("\n\rCode Start\n\r");
Pabs44 6:dfc31e7e2c23 145 SPCoff();
Pabs44 6:dfc31e7e2c23 146 Wheel.Period_in_ms(2);//Set frequency of the PWMs 500Hz
Pabs44 6:dfc31e7e2c23 147
Pabs44 6:dfc31e7e2c23 148 //CODE START --------------------------------------------
Pabs44 6:dfc31e7e2c23 149 double Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 150 wait(2.0);
Pabs44 6:dfc31e7e2c23 151 while(1){
Pabs44 6:dfc31e7e2c23 152 do{
Pabs44 6:dfc31e7e2c23 153 Wheel.Speed(0.7,0.7);//Turn left 70%
Pabs44 6:dfc31e7e2c23 154 wait(0.3);
Pabs44 6:dfc31e7e2c23 155 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 156 wait(0.1);
Pabs44 6:dfc31e7e2c23 157 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 158 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 159 wait(0.1);
Pabs44 6:dfc31e7e2c23 160 }while(Distance > 800);
Pabs44 6:dfc31e7e2c23 161
Pabs44 6:dfc31e7e2c23 162 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 163 wait(0.1);
Pabs44 6:dfc31e7e2c23 164 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 165 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 166 wait(0.1);
Pabs44 6:dfc31e7e2c23 167
Pabs44 6:dfc31e7e2c23 168 while(Distance > 90){
Pabs44 6:dfc31e7e2c23 169 Wheel.Speed(0.8,-0.8);//Forward 80%
Pabs44 6:dfc31e7e2c23 170 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 171 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 172 if(Distance > 950){
Pabs44 6:dfc31e7e2c23 173 while(Distance > 950){
Pabs44 6:dfc31e7e2c23 174 Wheel.Speed(-0.7,-0.7);//Turn left 70%
Pabs44 6:dfc31e7e2c23 175 wait(0.2);
Pabs44 6:dfc31e7e2c23 176 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 177 wait(0.1);
Pabs44 6:dfc31e7e2c23 178 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 179 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 180 wait(0.1);
Pabs44 6:dfc31e7e2c23 181 Wheel.Speed(0.7,0.7);//Turn left 70%
Pabs44 6:dfc31e7e2c23 182 wait(0.4);
Pabs44 6:dfc31e7e2c23 183 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 184 wait(0.1);
Pabs44 6:dfc31e7e2c23 185 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 186 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 187 wait(0.1);
Pabs44 6:dfc31e7e2c23 188 }
Pabs44 6:dfc31e7e2c23 189 }
Pabs44 6:dfc31e7e2c23 190 wait(0.2);
Pabs44 6:dfc31e7e2c23 191 }
Pabs44 6:dfc31e7e2c23 192
Pabs44 6:dfc31e7e2c23 193 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 194 wait(0.5);
Pabs44 6:dfc31e7e2c23 195 SPCtest();
Pabs44 6:dfc31e7e2c23 196 wait(0.5);
Pabs44 6:dfc31e7e2c23 197 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 198 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 199 wait(0.5);
Pabs44 6:dfc31e7e2c23 200
Pabs44 6:dfc31e7e2c23 201 Wheel.Speed(-0.8,0.8);
Pabs44 6:dfc31e7e2c23 202 wait(2.5);
Pabs44 6:dfc31e7e2c23 203 Wheel.Speed(0.7,0.7);
Pabs44 6:dfc31e7e2c23 204 wait(2.0);
Pabs44 6:dfc31e7e2c23 205 Wheel.Stop();
Pabs44 6:dfc31e7e2c23 206 ultra_sonic_distance();
Pabs44 6:dfc31e7e2c23 207 Distance = GetDistance();
Pabs44 6:dfc31e7e2c23 208 wait(0.2);
Pabs44 6:dfc31e7e2c23 209 }
awmcdowall 5:2621fc2ed6da 210 }