Luuk Lomillos Rozeboom / Mbed 2 deprecated RobotControlFinal

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.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@1:14b685c3abbd, 2017-10-12 (annotated)

Committer:
Luuk
Date:
Thu Oct 12 21:01:29 2017 +0000
Revision:
1:14b685c3abbd
Parent:
0:1b8a08e9a66c
Child:
2:e36213affbda
Selection of functions working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luuk 0:1b8a08e9a66c 1 #include "mbed.h"
Luuk 1:14b685c3abbd 2 #include "QEI.h"
Luuk 0:1b8a08e9a66c 3 #include "MODSERIAL.h"
Luuk 0:1b8a08e9a66c 4 //#include "HIDScope.h"
Luuk 0:1b8a08e9a66c 5
Luuk 0:1b8a08e9a66c 6
Luuk 0:1b8a08e9a66c 7 Serial pc (USBTX,USBRX);
Luuk 1:14b685c3abbd 8
Luuk 1:14b685c3abbd 9 PwmOut Motor1Vel(D6); //motor 1 velocity control
Luuk 1:14b685c3abbd 10 DigitalOut Motor1Dir(D7); //motor 1 direction
Luuk 1:14b685c3abbd 11 PwmOut Motor2Vel(D5); //motor 2 velocity control
Luuk 1:14b685c3abbd 12 DigitalOut Motor2Dir(D4); //motor 2 direction
Luuk 1:14b685c3abbd 13 DigitalOut led1(D2);
Luuk 1:14b685c3abbd 14 DigitalOut led2(D3);
Luuk 0:1b8a08e9a66c 15 AnalogIn emgx(A0);
Luuk 0:1b8a08e9a66c 16 AnalogIn emgy(A1);
Luuk 1:14b685c3abbd 17 //DigitalOut Button(D10);
Luuk 0:1b8a08e9a66c 18 //DigitalOut ledb(LED1);
Luuk 0:1b8a08e9a66c 19
Luuk 1:14b685c3abbd 20 QEI encoder1(D8,D9,NC,16);
Luuk 1:14b685c3abbd 21 QEI encoder2(D10,D11,NC,16);
Luuk 1:14b685c3abbd 22
Luuk 0:1b8a08e9a66c 23 Ticker switch_tick;
Luuk 1:14b685c3abbd 24 const float pi = 3.14159265359; //to convert pulses to radians
Luuk 1:14b685c3abbd 25 const float PulsesPerRev = 16; // ||
Luuk 1:14b685c3abbd 26 const float GearRatio = 131.15; // ||
Luuk 1:14b685c3abbd 27 const float Time = 0.01; //to control the tickers
Luuk 0:1b8a08e9a66c 28
Luuk 0:1b8a08e9a66c 29 float limitx =3, limity = 3, positionx = 1, positiony = 1;
Luuk 0:1b8a08e9a66c 30
Luuk 0:1b8a08e9a66c 31 int i = 0; //counter to go back to initial position if there is no activity
Luuk 0:1b8a08e9a66c 32
Luuk 0:1b8a08e9a66c 33 void movex()
Luuk 1:14b685c3abbd 34 { //move motor in the positive x axis
Luuk 0:1b8a08e9a66c 35 led1 = 1;
Luuk 0:1b8a08e9a66c 36 }
Luuk 0:1b8a08e9a66c 37 void movey()
Luuk 1:14b685c3abbd 38 { //move motor in the y axis
Luuk 0:1b8a08e9a66c 39 led2 = 1;
Luuk 0:1b8a08e9a66c 40 }
Luuk 0:1b8a08e9a66c 41 void movexy()
Luuk 1:14b685c3abbd 42 { //move motor in the x and y axis
Luuk 0:1b8a08e9a66c 43 led2 = 1;
Luuk 0:1b8a08e9a66c 44 led1 = 1;
Luuk 0:1b8a08e9a66c 45 }
Luuk 0:1b8a08e9a66c 46 void initialPosition()
Luuk 1:14b685c3abbd 47 { //move the motor back to the initial position
Luuk 0:1b8a08e9a66c 48 led2 = 0;
Luuk 0:1b8a08e9a66c 49 led1 = 0;
Luuk 0:1b8a08e9a66c 50 }
Luuk 0:1b8a08e9a66c 51 int choosecase(float emgx, float emgy)
Luuk 1:14b685c3abbd 52 { //decide which case has to be used
Luuk 0:1b8a08e9a66c 53 int a = 0;
Luuk 1:14b685c3abbd 54 if (positionx >= limitx || positiony >= limity) // limit so that the robot does not break. Maybe try approximating with an elipse.
Luuk 1:14b685c3abbd 55 {
Luuk 1:14b685c3abbd 56 a = 4;
Luuk 1:14b685c3abbd 57 i = 0;
Luuk 1:14b685c3abbd 58 }
Luuk 1:14b685c3abbd 59 else if (emgx > 0 && emgy == 0)
Luuk 0:1b8a08e9a66c 60 {
Luuk 0:1b8a08e9a66c 61 a = 1;
Luuk 0:1b8a08e9a66c 62 i = 0;
Luuk 0:1b8a08e9a66c 63 }
Luuk 0:1b8a08e9a66c 64 else if (emgx == 0 && emgy > 0)
Luuk 0:1b8a08e9a66c 65 {
Luuk 0:1b8a08e9a66c 66 a = 2;
Luuk 0:1b8a08e9a66c 67 i = 0;
Luuk 0:1b8a08e9a66c 68 }
Luuk 0:1b8a08e9a66c 69 else if (emgx > 0 && emgy > 0)
Luuk 0:1b8a08e9a66c 70 {
Luuk 0:1b8a08e9a66c 71 a = 3;
Luuk 0:1b8a08e9a66c 72 i = 0;
Luuk 0:1b8a08e9a66c 73 }
Luuk 1:14b685c3abbd 74 else if (i > 199) // after some time of inactivity (2s) the motor goes to initial position
Luuk 0:1b8a08e9a66c 75 {
Luuk 0:1b8a08e9a66c 76 a = 4;
Luuk 0:1b8a08e9a66c 77 i = 0;
Luuk 0:1b8a08e9a66c 78 }
Luuk 0:1b8a08e9a66c 79 else
Luuk 0:1b8a08e9a66c 80 {
Luuk 0:1b8a08e9a66c 81 i++;
Luuk 0:1b8a08e9a66c 82 }
Luuk 1:14b685c3abbd 83 //pc.printf("emgx: %f emgy: %f \r\n",emgx,emgy);
Luuk 0:1b8a08e9a66c 84 return a;
Luuk 0:1b8a08e9a66c 85
Luuk 0:1b8a08e9a66c 86 }
Luuk 0:1b8a08e9a66c 87 void selectcase()
Luuk 1:14b685c3abbd 88 { //call function to move the motors
Luuk 0:1b8a08e9a66c 89 int switchcons = choosecase(emgx,emgy);
Luuk 0:1b8a08e9a66c 90 switch(switchcons)
Luuk 0:1b8a08e9a66c 91 {
Luuk 0:1b8a08e9a66c 92 case 1:
Luuk 0:1b8a08e9a66c 93 movex();
Luuk 0:1b8a08e9a66c 94 break;
Luuk 0:1b8a08e9a66c 95 case 2:
Luuk 0:1b8a08e9a66c 96 movey();
Luuk 0:1b8a08e9a66c 97 break;
Luuk 0:1b8a08e9a66c 98 case 3:
Luuk 0:1b8a08e9a66c 99 movexy();
Luuk 0:1b8a08e9a66c 100 break;
Luuk 0:1b8a08e9a66c 101 case 4:
Luuk 0:1b8a08e9a66c 102 initialPosition();
Luuk 0:1b8a08e9a66c 103 break;
Luuk 0:1b8a08e9a66c 104 default:
Luuk 0:1b8a08e9a66c 105 break;
Luuk 0:1b8a08e9a66c 106 }
Luuk 0:1b8a08e9a66c 107 pc.printf("Chosen case: %d \r\n",switchcons);
Luuk 0:1b8a08e9a66c 108 }
Luuk 0:1b8a08e9a66c 109 main()
Luuk 0:1b8a08e9a66c 110 {
Luuk 0:1b8a08e9a66c 111 pc.baud(115200);
Luuk 0:1b8a08e9a66c 112 switch_tick.attach(&selectcase,0.01);
Luuk 0:1b8a08e9a66c 113 while (true) {
Luuk 0:1b8a08e9a66c 114
Luuk 0:1b8a08e9a66c 115 }
Luuk 0:1b8a08e9a66c 116 }