Ingmar Loohuis / Mbed 2 deprecated MotorControl

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@6:6bc6ce1fe94e, 2016-10-20 (annotated)

Committer:
IngmarLoohuis
Date:
Thu Oct 20 10:15:28 2016 +0000
Revision:
6:6bc6ce1fe94e
Parent:
5:931594a366b7
Child:
7:742b1969f6c9
Gelimiteerde draai werkt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IngmarLoohuis 0:2f40eb89ffce 1 #include "mbed.h"
IngmarLoohuis 2:665df4abd084 2 #include "MODSERIAL.h"
IngmarLoohuis 0:2f40eb89ffce 3 #include "QEI.h"
IngmarLoohuis 0:2f40eb89ffce 4 #include "math.h"
IngmarLoohuis 5:931594a366b7 5 #include "HIDScope.h"
IngmarLoohuis 1:f26a53da33ed 6
IngmarLoohuis 6:6bc6ce1fe94e 7
IngmarLoohuis 6:6bc6ce1fe94e 8 //*****************Defining ports********************
IngmarLoohuis 2:665df4abd084 9 DigitalOut motor1DirectionPin (D4);
IngmarLoohuis 1:f26a53da33ed 10 PwmOut motor1MagnitudePin(D5);
IngmarLoohuis 2:665df4abd084 11 DigitalIn button(D2);
IngmarLoohuis 1:f26a53da33ed 12 Serial pc(USBTX,USBRX);
IngmarLoohuis 4:30d8610b63a6 13 QEI encoder(D12,D13,NC,32);
IngmarLoohuis 5:931594a366b7 14 HIDScope scope(1);
IngmarLoohuis 1:f26a53da33ed 15
IngmarLoohuis 6:6bc6ce1fe94e 16 //*******************Setting tickers and printers*******************
IngmarLoohuis 2:665df4abd084 17 Ticker callMotor;
IngmarLoohuis 6:6bc6ce1fe94e 18 Ticker angPos;
IngmarLoohuis 6:6bc6ce1fe94e 19 Ticker t1;
IngmarLoohuis 6:6bc6ce1fe94e 20
IngmarLoohuis 6:6bc6ce1fe94e 21 //**************Go flags********************************************
IngmarLoohuis 6:6bc6ce1fe94e 22 volatile bool fn1_go = false;
IngmarLoohuis 6:6bc6ce1fe94e 23 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
IngmarLoohuis 6:6bc6ce1fe94e 24
IngmarLoohuis 6:6bc6ce1fe94e 25 //***************Global Variables***********************************
IngmarLoohuis 6:6bc6ce1fe94e 26 const double pi = 3.14159265359;
IngmarLoohuis 6:6bc6ce1fe94e 27 const double ts = 1.0/1000.0;
IngmarLoohuis 6:6bc6ce1fe94e 28 const int velocityChannel = 0;
IngmarLoohuis 6:6bc6ce1fe94e 29 volatile double radians =0.0;
IngmarLoohuis 6:6bc6ce1fe94e 30 const double transmissionShoulder =94.4/40.2;
IngmarLoohuis 6:6bc6ce1fe94e 31 const double transmissionElbow = 1.0;
IngmarLoohuis 6:6bc6ce1fe94e 32 //*****************Angles Arms***********************
IngmarLoohuis 1:f26a53da33ed 33
IngmarLoohuis 6:6bc6ce1fe94e 34 double O1=1.7633;
IngmarLoohuis 6:6bc6ce1fe94e 35 double O2=2.0915;
IngmarLoohuis 6:6bc6ce1fe94e 36 double O3=1.8685;
IngmarLoohuis 6:6bc6ce1fe94e 37 double O4=1.1363;
IngmarLoohuis 6:6bc6ce1fe94e 38 double O5=2.3960;
IngmarLoohuis 6:6bc6ce1fe94e 39 double O6=2.0827;
IngmarLoohuis 6:6bc6ce1fe94e 40 double B1=1.3551;
IngmarLoohuis 6:6bc6ce1fe94e 41 double B2=0.5964;
IngmarLoohuis 6:6bc6ce1fe94e 42 double B3=0.06652;
IngmarLoohuis 6:6bc6ce1fe94e 43 double B4=0.0669;
IngmarLoohuis 6:6bc6ce1fe94e 44 double B5=1.7462;
IngmarLoohuis 6:6bc6ce1fe94e 45 double B6=-0.8994;
IngmarLoohuis 5:931594a366b7 46
IngmarLoohuis 6:6bc6ce1fe94e 47 //**********functions******************************
IngmarLoohuis 6:6bc6ce1fe94e 48 void getAngPosition()
IngmarLoohuis 6:6bc6ce1fe94e 49 {
IngmarLoohuis 6:6bc6ce1fe94e 50 volatile int pulses = encoder.getPulses();
IngmarLoohuis 6:6bc6ce1fe94e 51 radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
IngmarLoohuis 6:6bc6ce1fe94e 52 scope.set(velocityChannel,radians);
IngmarLoohuis 6:6bc6ce1fe94e 53 scope.send();
IngmarLoohuis 6:6bc6ce1fe94e 54 }
IngmarLoohuis 6:6bc6ce1fe94e 55
IngmarLoohuis 6:6bc6ce1fe94e 56 void control(double radians)
IngmarLoohuis 4:30d8610b63a6 57 {
IngmarLoohuis 6:6bc6ce1fe94e 58 if(radians>=(-2*pi))
IngmarLoohuis 2:665df4abd084 59 {
IngmarLoohuis 6:6bc6ce1fe94e 60 motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
IngmarLoohuis 2:665df4abd084 61 }
IngmarLoohuis 2:665df4abd084 62 else
IngmarLoohuis 2:665df4abd084 63 {
IngmarLoohuis 6:6bc6ce1fe94e 64 motor1MagnitudePin=0.0;
IngmarLoohuis 2:665df4abd084 65 }
IngmarLoohuis 0:2f40eb89ffce 66 }
IngmarLoohuis 6:6bc6ce1fe94e 67 //****************MAIN FUNCTION*********************************
IngmarLoohuis 0:2f40eb89ffce 68 int main()
IngmarLoohuis 2:665df4abd084 69 {
IngmarLoohuis 6:6bc6ce1fe94e 70 angPos.attach(&getAngPosition,ts);
IngmarLoohuis 6:6bc6ce1fe94e 71 t1.attach(&fn1_activate, 0.0001f);
IngmarLoohuis 6:6bc6ce1fe94e 72 while(true)
IngmarLoohuis 6:6bc6ce1fe94e 73 if(fn1_go)
IngmarLoohuis 6:6bc6ce1fe94e 74 {
IngmarLoohuis 6:6bc6ce1fe94e 75 fn1_go = false;
IngmarLoohuis 6:6bc6ce1fe94e 76 control(radians);
IngmarLoohuis 6:6bc6ce1fe94e 77 }
IngmarLoohuis 6:6bc6ce1fe94e 78
IngmarLoohuis 6:6bc6ce1fe94e 79
IngmarLoohuis 2:665df4abd084 80 pc.baud(115200);
IngmarLoohuis 6:6bc6ce1fe94e 81 while(true);
IngmarLoohuis 2:665df4abd084 82 }