Angus McDowall / Mbed 2 deprecated ROCO104_Ultrasound_SPC

pabs

Dependencies:   mbed motor

main.cpp@0:51c12cc34baf, 2018-02-01 (annotated)

Committer:
martinsimpson
Date:
Thu Feb 01 12:59:21 2018 +0000
Revision:
0:51c12cc34baf
Child:
1:3ca91ad8e927
First Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
martinsimpson 0:51c12cc34baf 1 /*
martinsimpson 0:51c12cc34baf 2 Simple Routine for Nucleo Board for ROCO103PP Buggy Motor COntrol and Microswitches
martinsimpson 0:51c12cc34baf 3 Plymouth University
martinsimpson 0:51c12cc34baf 4 M.Simpson 31st October 2016
martinsimpson 0:51c12cc34baf 5 Editted 03/02/2017
martinsimpson 0:51c12cc34baf 6 */
martinsimpson 0:51c12cc34baf 7 #include "mbed.h"
martinsimpson 0:51c12cc34baf 8 #include "motor.h"
martinsimpson 0:51c12cc34baf 9 #include "tunes.h"
martinsimpson 0:51c12cc34baf 10 #define TIME_PERIOD 2 //Constant compiler Values here 2 equates to 2ms or 500Hz base Frequency
martinsimpson 0:51c12cc34baf 11 #define DUTY 0.9 //DUTY of 1.0=100%, 0.4=40% etc.,
martinsimpson 0:51c12cc34baf 12
martinsimpson 0:51c12cc34baf 13 DigitalIn microswitch1(D4); //Instance of the DigitalIn class called 'microswitch1'
martinsimpson 0:51c12cc34baf 14 DigitalIn microswitch2(D3); //Instance of the DigitalIn class called 'microswitch2'
martinsimpson 0:51c12cc34baf 15
martinsimpson 0:51c12cc34baf 16 Motor motor_A(D7,D8); //Instances of the Motor Class see motor.h anf motor.cpp
martinsimpson 0:51c12cc34baf 17 Motor motor_B(D9,D10); //They must be connected to these ports D7,D8 & D9,D10
martinsimpson 0:51c12cc34baf 18
martinsimpson 0:51c12cc34baf 19 DigitalIn myButton(USER_BUTTON); //USER_BUTTON is the Blue Button on the NUCLEO Board
martinsimpson 0:51c12cc34baf 20
martinsimpson 0:51c12cc34baf 21 DigitalOut led(LED1); //LED1 is the Green LED on the NUCLEO board
martinsimpson 0:51c12cc34baf 22 //N.B. The RED LED is the POWER Indicator
martinsimpson 0:51c12cc34baf 23 //and the Multicoloured LED indicates status of the ST-LINK Programming cycle
martinsimpson 0:51c12cc34baf 24
martinsimpson 0:51c12cc34baf 25 Serial pc(USBTX,USBRX); //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
martinsimpson 0:51c12cc34baf 26 //This is Pseudo RS232 over USB the NUCLEO will appear as a COMx Port see device Manager on PC used
martinsimpson 0:51c12cc34baf 27 //Use PuTTY to monitor check COMx and BAUD rate (115200)
martinsimpson 0:51c12cc34baf 28
martinsimpson 0:51c12cc34baf 29 //The Following line is a Function Prototype
martinsimpson 0:51c12cc34baf 30 int motor(float speedA, float speedB); //call as motor('Speed of MotorA Left','Speed of MotorB Right')
martinsimpson 0:51c12cc34baf 31 //Where speed ranges from -1.0 to +1.0 inclusive rto give full reverse to full forward
martinsimpson 0:51c12cc34baf 32 //And of course 0.0 will stop the Motor
martinsimpson 0:51c12cc34baf 33
martinsimpson 0:51c12cc34baf 34 //Variable 'duty' for programmer to use to vary speed as required set here to #define compiler constant see above
martinsimpson 0:51c12cc34baf 35 float duty=DUTY;
martinsimpson 0:51c12cc34baf 36 //
martinsimpson 0:51c12cc34baf 37 int main ()
martinsimpson 0:51c12cc34baf 38 {
martinsimpson 0:51c12cc34baf 39 pc.baud(115200); //BAUD Rate to 115200
martinsimpson 0:51c12cc34baf 40 pc.printf("ROCO103PP Demonstration Robot Buggy Plymouth University 2016/17\n\r");
martinsimpson 0:51c12cc34baf 41
martinsimpson 0:51c12cc34baf 42 motor_A.Period_in_ms(TIME_PERIOD); //Set frequency of the PWMs
martinsimpson 0:51c12cc34baf 43 motor_B.Period_in_ms(TIME_PERIOD);
martinsimpson 0:51c12cc34baf 44 //
martinsimpson 0:51c12cc34baf 45 //--------------------------- your strategy goes between the two dashed lines ---------------------------------------------
martinsimpson 0:51c12cc34baf 46 //
martinsimpson 0:51c12cc34baf 47 motor(0.0f,0.0f); // Ensure Motors are stopped - For the curious, this function is defined at the end of this file.
martinsimpson 0:51c12cc34baf 48
martinsimpson 0:51c12cc34baf 49 close_encounter(1); //tune to play Announce start!
martinsimpson 0:51c12cc34baf 50 //twinkle(1); //see tunes.h for alternatives or make your own!
martinsimpson 0:51c12cc34baf 51 //jingle_bells(1);
martinsimpson 0:51c12cc34baf 52 while(myButton==1)
martinsimpson 0:51c12cc34baf 53 { //Wait here for USER Button (Blue) on Nucleo Board (goes to zero when pressed)
martinsimpson 0:51c12cc34baf 54 led=0; //and flash green LED whilst waiting
martinsimpson 0:51c12cc34baf 55 wait(0.1);
martinsimpson 0:51c12cc34baf 56 led=1;
martinsimpson 0:51c12cc34baf 57 wait(0.1);
martinsimpson 0:51c12cc34baf 58 if(microswitch1==1)
martinsimpson 0:51c12cc34baf 59 {
martinsimpson 0:51c12cc34baf 60 pc.printf("Switch1 = %4.2fV\n\r",(float)microswitch1*3.3f);//printing value of microswitch1 in PuTTy window on PC
martinsimpson 0:51c12cc34baf 61 //NB this is a Digital Input and so returns a Boolean 1 or 0
martinsimpson 0:51c12cc34baf 62 //and so 'cast' the result into a 'float' type and multiply by 3.3!
martinsimpson 0:51c12cc34baf 63 // see the instruction doc on how to install putty.
martinsimpson 0:51c12cc34baf 64 tone1();
martinsimpson 0:51c12cc34baf 65 }
martinsimpson 0:51c12cc34baf 66 //Test Microswitches with two different tones see tunes.cpp tunes.h
martinsimpson 0:51c12cc34baf 67 if(microswitch2==1)
martinsimpson 0:51c12cc34baf 68 {
martinsimpson 0:51c12cc34baf 69 pc.printf("Switch 2 pressed\n\r"); //Another example of how to print a message telling about the program workings.
martinsimpson 0:51c12cc34baf 70 tone2();
martinsimpson 0:51c12cc34baf 71 }
martinsimpson 0:51c12cc34baf 72 }
martinsimpson 0:51c12cc34baf 73
martinsimpson 0:51c12cc34baf 74 while(true) //Repeat the following forever
martinsimpson 0:51c12cc34baf 75 {
martinsimpson 0:51c12cc34baf 76 motor(duty,duty); //Start Moving forward
martinsimpson 0:51c12cc34baf 77
martinsimpson 0:51c12cc34baf 78 while(microswitch1==0&&microswitch2==0){wait(0.05);} //short delay for debounce/noise
martinsimpson 0:51c12cc34baf 79
martinsimpson 0:51c12cc34baf 80 motor(0,0); //STOP Motors
martinsimpson 0:51c12cc34baf 81 wait(0.1); //Allow time for motors to stop
martinsimpson 0:51c12cc34baf 82
martinsimpson 0:51c12cc34baf 83 if(microswitch1==1) //Execute the following code if microswitch1 is activated
martinsimpson 0:51c12cc34baf 84 {
martinsimpson 0:51c12cc34baf 85 motor(0.0f,0.0f); //Stop the Motors
martinsimpson 0:51c12cc34baf 86 tone1();
martinsimpson 0:51c12cc34baf 87 motor(-duty,-duty);
martinsimpson 0:51c12cc34baf 88 wait(2.0f);
martinsimpson 0:51c12cc34baf 89 motor(0,0);
martinsimpson 0:51c12cc34baf 90 wait(1.0f);
martinsimpson 0:51c12cc34baf 91 motor(-duty,duty);
martinsimpson 0:51c12cc34baf 92 wait(2.0f);
martinsimpson 0:51c12cc34baf 93 motor(0,0);
martinsimpson 0:51c12cc34baf 94 wait(1.0f);
martinsimpson 0:51c12cc34baf 95 }
martinsimpson 0:51c12cc34baf 96
martinsimpson 0:51c12cc34baf 97 if(microswitch2==1) //Execute the following code if microswitch2 is activated
martinsimpson 0:51c12cc34baf 98 {
martinsimpson 0:51c12cc34baf 99 motor(0.0f,0.0f); //Stop the Motors
martinsimpson 0:51c12cc34baf 100 tone2();
martinsimpson 0:51c12cc34baf 101 motor(-duty,-duty);
martinsimpson 0:51c12cc34baf 102 wait(2.0f);
martinsimpson 0:51c12cc34baf 103 motor(0,0);
martinsimpson 0:51c12cc34baf 104 wait(1.0f);
martinsimpson 0:51c12cc34baf 105 motor(duty,-duty);
martinsimpson 0:51c12cc34baf 106 wait(2.0f);
martinsimpson 0:51c12cc34baf 107 motor(0,0);
martinsimpson 0:51c12cc34baf 108 wait(1.0f);
martinsimpson 0:51c12cc34baf 109 }
martinsimpson 0:51c12cc34baf 110 }
martinsimpson 0:51c12cc34baf 111 }
martinsimpson 0:51c12cc34baf 112 //
martinsimpson 0:51c12cc34baf 113 //----------------------------------------------------------------------------------------------
martinsimpson 0:51c12cc34baf 114 //
martinsimpson 0:51c12cc34baf 115
martinsimpson 0:51c12cc34baf 116 //Small function to control motors use as motor(1.0,-0.5) Motor A full speed forward Motor B half speed reversed
martinsimpson 0:51c12cc34baf 117 int motor(float speedA, float speedB){
martinsimpson 0:51c12cc34baf 118 if(speedA>1.0f||speedA<-1.0f){ //CHECK speedA Value is in Range!
martinsimpson 0:51c12cc34baf 119 return -1; //return ERROR code -1=speedA Value out of range! EXIT Function
martinsimpson 0:51c12cc34baf 120 }
martinsimpson 0:51c12cc34baf 121 if(speedB>1.0f||speedA<-1.0f){ //CHECK speedB Value is in Range!
martinsimpson 0:51c12cc34baf 122 return -2; //return ERROR code -2=speedB Value out of range! EXIT Function
martinsimpson 0:51c12cc34baf 123 }
martinsimpson 0:51c12cc34baf 124 //If speed values have passed the checks above then the following code will be executed
martinsimpson 0:51c12cc34baf 125 if(speedA<0.0f){
martinsimpson 0:51c12cc34baf 126 motor_A.Rev(-speedA);
martinsimpson 0:51c12cc34baf 127 }
martinsimpson 0:51c12cc34baf 128 else{
martinsimpson 0:51c12cc34baf 129 motor_A.Fwd(speedA);
martinsimpson 0:51c12cc34baf 130 }
martinsimpson 0:51c12cc34baf 131 if(speedB<0.0f){
martinsimpson 0:51c12cc34baf 132 motor_B.Rev(-speedB);
martinsimpson 0:51c12cc34baf 133 }
martinsimpson 0:51c12cc34baf 134 else{
martinsimpson 0:51c12cc34baf 135 motor_B.Fwd(speedB);
martinsimpson 0:51c12cc34baf 136 }
martinsimpson 0:51c12cc34baf 137 return 0; //Return ERROR code Zero i.e. NO ERROR success!
martinsimpson 0:51c12cc34baf 138 }
martinsimpson 0:51c12cc34baf 139
martinsimpson 0:51c12cc34baf 140 /* //Consider these lines of code to Accelerate the motors
martinsimpson 0:51c12cc34baf 141 // for (float i=0.5f; i<=1.0f; i+=0.01f) //Accelerate from 50% to 100%
martinsimpson 0:51c12cc34baf 142 // {
martinsimpson 0:51c12cc34baf 143 // motor(i,i);
martinsimpson 0:51c12cc34baf 144 // wait(0.1f);
martinsimpson 0:51c12cc34baf 145 // }
martinsimpson 0:51c12cc34baf 146 */