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@2:e36213affbda, 2017-10-15 (annotated)

Committer:
Luuk
Date:
Sun Oct 15 14:15:10 2017 +0000
Revision:
2:e36213affbda
Parent:
1:14b685c3abbd
Child:
3:03db694efdbe
Working program. The work space approximation has to be improved.

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 2:e36213affbda 14 AnalogIn emg1(A0);
Luuk 2:e36213affbda 15 AnalogIn emg2(A1);
Luuk 2:e36213affbda 16 DigitalOut Button(D3);
Luuk 0:1b8a08e9a66c 17 //DigitalOut ledb(LED1);
Luuk 0:1b8a08e9a66c 18
Luuk 1:14b685c3abbd 19 QEI encoder1(D8,D9,NC,16);
Luuk 1:14b685c3abbd 20 QEI encoder2(D10,D11,NC,16);
Luuk 1:14b685c3abbd 21
Luuk 0:1b8a08e9a66c 22 Ticker switch_tick;
Luuk 2:e36213affbda 23 float Angle1,Angle2; //real angles of the motor
Luuk 2:e36213affbda 24 const float Angle1_0 = 0.8411 ,Angle2_0 = -0.8411; //initial angle of the motors
Luuk 2:e36213affbda 25 float XPosition = 0.0, YPosition = 0.0; //real position of end point
Luuk 2:e36213affbda 26 float L2 = 300.0;
Luuk 2:e36213affbda 27 float L3 = 300.0; //length of the arms of the robot
Luuk 1:14b685c3abbd 28 const float pi = 3.14159265359; //to convert pulses to radians
Luuk 2:e36213affbda 29 const float PulsesPerRev = 16.0; // ||
Luuk 1:14b685c3abbd 30 const float GearRatio = 131.15; // ||
Luuk 1:14b685c3abbd 31 const float Time = 0.01; //to control the tickers
Luuk 0:1b8a08e9a66c 32
Luuk 2:e36213affbda 33 float R = 160.0; //radius of the work space
Luuk 2:e36213affbda 34
Luuk 0:1b8a08e9a66c 35
Luuk 2:e36213affbda 36 void positionxy(float Angle1, float Angle2,float &XPosition,float &YPosition)
Luuk 2:e36213affbda 37 {
Luuk 2:e36213affbda 38 XPosition = L2*cos(Angle1)+L3*cos(Angle2);
Luuk 2:e36213affbda 39 YPosition = L2*sin(Angle1)+L3*sin(Angle2);
Luuk 2:e36213affbda 40 }
Luuk 0:1b8a08e9a66c 41
Luuk 2:e36213affbda 42 void moveXpYp()
Luuk 2:e36213affbda 43 {
Luuk 2:e36213affbda 44 Motor1Vel = emg1*2-1;
Luuk 2:e36213affbda 45 Motor2Vel = emg2*2-1;
Luuk 2:e36213affbda 46 Motor1Dir = 1;
Luuk 2:e36213affbda 47 Motor2Dir = 1;
Luuk 0:1b8a08e9a66c 48 }
Luuk 2:e36213affbda 49 void moveXpYn()
Luuk 2:e36213affbda 50 {
Luuk 2:e36213affbda 51 Motor1Vel = emg1*2-1;
Luuk 2:e36213affbda 52 Motor2Vel = 1-emg2*2;
Luuk 2:e36213affbda 53 Motor1Dir = 1;
Luuk 2:e36213affbda 54 Motor2Dir = 0;
Luuk 2:e36213affbda 55 }
Luuk 2:e36213affbda 56 void moveXnYp()
Luuk 2:e36213affbda 57 {
Luuk 2:e36213affbda 58 Motor1Vel = 1-emg1*2;
Luuk 2:e36213affbda 59 Motor2Vel = emg2*2-1;
Luuk 2:e36213affbda 60 Motor1Dir = 0;
Luuk 2:e36213affbda 61 Motor2Dir = 1;
Luuk 0:1b8a08e9a66c 62 }
Luuk 2:e36213affbda 63 void moveXnYn()
Luuk 2:e36213affbda 64 {
Luuk 2:e36213affbda 65 Motor1Vel = 1-emg1*2;
Luuk 2:e36213affbda 66 Motor2Vel = 1-emg2*2;
Luuk 2:e36213affbda 67 Motor1Dir = 0;
Luuk 2:e36213affbda 68 Motor2Dir = 0;
Luuk 2:e36213affbda 69 }
Luuk 2:e36213affbda 70 void StopMotors()
Luuk 2:e36213affbda 71 {
Luuk 2:e36213affbda 72 Motor1Vel = 0;
Luuk 2:e36213affbda 73 Motor2Vel = 0;
Luuk 0:1b8a08e9a66c 74 }
Luuk 2:e36213affbda 75 int choosecase(float cons1, float cons2)
Luuk 1:14b685c3abbd 76 { //decide which case has to be used
Luuk 2:e36213affbda 77 Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0; //angles of the motors in radians
Luuk 2:e36213affbda 78 Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
Luuk 2:e36213affbda 79 positionxy(Angle1,Angle2,XPosition,YPosition); //calculate the position of the end point
Luuk 0:1b8a08e9a66c 80 int a = 0;
Luuk 2:e36213affbda 81 if (Button == 1)
Luuk 2:e36213affbda 82 {
Luuk 2:e36213affbda 83 // not do anything
Luuk 2:e36213affbda 84 }
Luuk 2:e36213affbda 85 else if (pow((XPosition-400),2) + pow(YPosition,2) > pow(R,2)) // limit so that the robot does not break. Maybe try approximating with an elipse.
Luuk 2:e36213affbda 86 {
Luuk 2:e36213affbda 87 a = 1; // don't move, the robot has reached the end of the workspace
Luuk 2:e36213affbda 88 }
Luuk 2:e36213affbda 89 else if (cons1*10 >= 5 && cons2*10 >= 5)
Luuk 2:e36213affbda 90 {
Luuk 2:e36213affbda 91 a = 2;
Luuk 2:e36213affbda 92 }
Luuk 2:e36213affbda 93 else if (cons1*10 >= 5 && cons2*10 < 5)
Luuk 2:e36213affbda 94 {
Luuk 2:e36213affbda 95 a = 3;
Luuk 2:e36213affbda 96 }
Luuk 2:e36213affbda 97 else if (cons1*10 < 5 && cons2*10 >= 5)
Luuk 1:14b685c3abbd 98 {
Luuk 1:14b685c3abbd 99 a = 4;
Luuk 2:e36213affbda 100 }
Luuk 2:e36213affbda 101 else if (cons1*10 < 5 && cons2*10 < 5)
Luuk 0:1b8a08e9a66c 102 {
Luuk 2:e36213affbda 103 a = 5;
Luuk 0:1b8a08e9a66c 104 }
Luuk 2:e36213affbda 105 pc.printf("case: %d \r\n",a);
Luuk 0:1b8a08e9a66c 106 return a;
Luuk 0:1b8a08e9a66c 107
Luuk 0:1b8a08e9a66c 108 }
Luuk 0:1b8a08e9a66c 109 void selectcase()
Luuk 1:14b685c3abbd 110 { //call function to move the motors
Luuk 2:e36213affbda 111 int switchcons = choosecase(emg1.read(),emg2.read());
Luuk 0:1b8a08e9a66c 112 switch(switchcons)
Luuk 0:1b8a08e9a66c 113 {
Luuk 0:1b8a08e9a66c 114 case 1:
Luuk 2:e36213affbda 115 StopMotors();
Luuk 0:1b8a08e9a66c 116 break;
Luuk 0:1b8a08e9a66c 117 case 2:
Luuk 2:e36213affbda 118 moveXpYp();
Luuk 0:1b8a08e9a66c 119 break;
Luuk 0:1b8a08e9a66c 120 case 3:
Luuk 2:e36213affbda 121 moveXpYn();
Luuk 0:1b8a08e9a66c 122 break;
Luuk 0:1b8a08e9a66c 123 case 4:
Luuk 2:e36213affbda 124 moveXnYp();
Luuk 2:e36213affbda 125 break;
Luuk 2:e36213affbda 126 case 5:
Luuk 2:e36213affbda 127 moveXnYn();
Luuk 0:1b8a08e9a66c 128 break;
Luuk 0:1b8a08e9a66c 129 default:
Luuk 0:1b8a08e9a66c 130 break;
Luuk 0:1b8a08e9a66c 131 }
Luuk 2:e36213affbda 132
Luuk 0:1b8a08e9a66c 133 }
Luuk 0:1b8a08e9a66c 134 main()
Luuk 0:1b8a08e9a66c 135 {
Luuk 2:e36213affbda 136 Motor1Dir = 1;
Luuk 2:e36213affbda 137 Motor2Dir = 1;
Luuk 0:1b8a08e9a66c 138 pc.baud(115200);
Luuk 2:e36213affbda 139 switch_tick.attach(&selectcase,Time);
Luuk 2:e36213affbda 140 while (true)
Luuk 2:e36213affbda 141 {
Luuk 2:e36213affbda 142 pc.printf("XP: %f YP: %f EMG1: %f EMG2: %f\r\n", XPosition, YPosition,emg1.read(),emg2.read());
Luuk 2:e36213affbda 143 wait(0.5f);
Luuk 0:1b8a08e9a66c 144 }
Luuk 0:1b8a08e9a66c 145 }