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@10:54b66bd1db20, 2016-10-21 (annotated)

Committer:
IngmarLoohuis
Date:
Fri Oct 21 10:27:04 2016 +0000
Revision:
10:54b66bd1db20
Parent:
9:6a065971d0ae
Child:
11:eda4fbf91948
''

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 8:7cc4d6d9c2b5 15 DigitalOut led(D0);
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 7:742b1969f6c9 20 Ticker t2;
IngmarLoohuis 7:742b1969f6c9 21 Ticker t3;
IngmarLoohuis 7:742b1969f6c9 22 Ticker myControllerTicker;
IngmarLoohuis 6:6bc6ce1fe94e 23 //**************Go flags********************************************
IngmarLoohuis 6:6bc6ce1fe94e 24 volatile bool fn1_go = false;
IngmarLoohuis 6:6bc6ce1fe94e 25 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
IngmarLoohuis 7:742b1969f6c9 26 volatile bool fn2_go = false;
IngmarLoohuis 7:742b1969f6c9 27 void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
IngmarLoohuis 7:742b1969f6c9 28 volatile bool fn3_go = false;
IngmarLoohuis 7:742b1969f6c9 29 void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
IngmarLoohuis 6:6bc6ce1fe94e 30
IngmarLoohuis 6:6bc6ce1fe94e 31 //***************Global Variables***********************************
IngmarLoohuis 6:6bc6ce1fe94e 32 const double pi = 3.14159265359;
IngmarLoohuis 6:6bc6ce1fe94e 33 const double ts = 1.0/1000.0;
IngmarLoohuis 6:6bc6ce1fe94e 34 const int velocityChannel = 0;
IngmarLoohuis 6:6bc6ce1fe94e 35 const double transmissionShoulder =94.4/40.2;
IngmarLoohuis 6:6bc6ce1fe94e 36 const double transmissionElbow = 1.0;
IngmarLoohuis 7:742b1969f6c9 37 const double MOTOR1_KP = 2.5;
IngmarLoohuis 9:6a065971d0ae 38 const double MOTOR1_KI = 1.0;
IngmarLoohuis 7:742b1969f6c9 39 const double CONTROLLER_TS = 0.01;
IngmarLoohuis 9:6a065971d0ae 40 double m1_err_int=0;
IngmarLoohuis 8:7cc4d6d9c2b5 41 volatile double radians;
IngmarLoohuis 8:7cc4d6d9c2b5 42 volatile double proportionalErrormotor1;
IngmarLoohuis 9:6a065971d0ae 43 volatile double motor1;
IngmarLoohuis 6:6bc6ce1fe94e 44 //*****************Angles Arms***********************
IngmarLoohuis 1:f26a53da33ed 45
IngmarLoohuis 6:6bc6ce1fe94e 46 double O1=1.7633;
IngmarLoohuis 6:6bc6ce1fe94e 47 double O2=2.0915;
IngmarLoohuis 6:6bc6ce1fe94e 48 double O3=1.8685;
IngmarLoohuis 6:6bc6ce1fe94e 49 double O4=1.1363;
IngmarLoohuis 6:6bc6ce1fe94e 50 double O5=2.3960;
IngmarLoohuis 6:6bc6ce1fe94e 51 double O6=2.0827;
IngmarLoohuis 6:6bc6ce1fe94e 52 double B1=1.3551;
IngmarLoohuis 6:6bc6ce1fe94e 53 double B2=0.5964;
IngmarLoohuis 6:6bc6ce1fe94e 54 double B3=0.06652;
IngmarLoohuis 6:6bc6ce1fe94e 55 double B4=0.0669;
IngmarLoohuis 6:6bc6ce1fe94e 56 double B5=1.7462;
IngmarLoohuis 6:6bc6ce1fe94e 57 double B6=-0.8994;
IngmarLoohuis 5:931594a366b7 58
IngmarLoohuis 6:6bc6ce1fe94e 59 //**********functions******************************
IngmarLoohuis 9:6a065971d0ae 60 double P(volatile double error, const double Kp){
IngmarLoohuis 9:6a065971d0ae 61 return Kp * error;
IngmarLoohuis 9:6a065971d0ae 62 }
IngmarLoohuis 9:6a065971d0ae 63
IngmarLoohuis 8:7cc4d6d9c2b5 64 void getAngPosition()
IngmarLoohuis 6:6bc6ce1fe94e 65 {
IngmarLoohuis 6:6bc6ce1fe94e 66 volatile int pulses = encoder.getPulses();
IngmarLoohuis 9:6a065971d0ae 67 radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
IngmarLoohuis 6:6bc6ce1fe94e 68 }
IngmarLoohuis 6:6bc6ce1fe94e 69
IngmarLoohuis 7:742b1969f6c9 70 // Next task, measure the error and apply the output to the plant
IngmarLoohuis 8:7cc4d6d9c2b5 71 void motor1_Controller(double radians)
IngmarLoohuis 7:742b1969f6c9 72 {
IngmarLoohuis 10:54b66bd1db20 73 double reference = 2*pi;
IngmarLoohuis 8:7cc4d6d9c2b5 74 volatile double error1 = reference - radians;
IngmarLoohuis 9:6a065971d0ae 75 motor1 = P(error1,MOTOR1_KP) ;
IngmarLoohuis 9:6a065971d0ae 76 // scope.set(velocityChannel,proportionalErrormotor1);
IngmarLoohuis 9:6a065971d0ae 77 //scope.send();
IngmarLoohuis 7:742b1969f6c9 78 }
IngmarLoohuis 7:742b1969f6c9 79
IngmarLoohuis 7:742b1969f6c9 80
IngmarLoohuis 10:54b66bd1db20 81 void control(double motor1)
IngmarLoohuis 4:30d8610b63a6 82 {
IngmarLoohuis 9:6a065971d0ae 83 if(abs(motor1)>0.3)
IngmarLoohuis 2:665df4abd084 84 {
IngmarLoohuis 9:6a065971d0ae 85 motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
IngmarLoohuis 2:665df4abd084 86 }
IngmarLoohuis 9:6a065971d0ae 87 else
IngmarLoohuis 2:665df4abd084 88 {
IngmarLoohuis 6:6bc6ce1fe94e 89 motor1MagnitudePin=0.0;
IngmarLoohuis 2:665df4abd084 90 }
IngmarLoohuis 9:6a065971d0ae 91 if(motor1<=0)
IngmarLoohuis 9:6a065971d0ae 92 {
IngmarLoohuis 9:6a065971d0ae 93 motor1DirectionPin=0.0;
IngmarLoohuis 9:6a065971d0ae 94 }
IngmarLoohuis 9:6a065971d0ae 95 else {
IngmarLoohuis 9:6a065971d0ae 96 motor1DirectionPin=1.0;
IngmarLoohuis 9:6a065971d0ae 97 }
IngmarLoohuis 0:2f40eb89ffce 98 }
IngmarLoohuis 7:742b1969f6c9 99
IngmarLoohuis 6:6bc6ce1fe94e 100 //****************MAIN FUNCTION*********************************
IngmarLoohuis 0:2f40eb89ffce 101 int main()
IngmarLoohuis 8:7cc4d6d9c2b5 102 {motor1MagnitudePin.period(1.0/1000.0);
IngmarLoohuis 8:7cc4d6d9c2b5 103 t1.attach(&fn1_activate, 0.1f);
IngmarLoohuis 7:742b1969f6c9 104 t2.attach(&fn2_activate, 0.0001f);
IngmarLoohuis 7:742b1969f6c9 105 t3.attach(&fn3_activate, 0.0001f);
IngmarLoohuis 10:54b66bd1db20 106
IngmarLoohuis 10:54b66bd1db20 107 while(true)
IngmarLoohuis 10:54b66bd1db20 108 {
IngmarLoohuis 6:6bc6ce1fe94e 109 if(fn1_go)
IngmarLoohuis 6:6bc6ce1fe94e 110 {
IngmarLoohuis 6:6bc6ce1fe94e 111 fn1_go = false;
IngmarLoohuis 10:54b66bd1db20 112 control(motor1);
IngmarLoohuis 6:6bc6ce1fe94e 113 }
IngmarLoohuis 7:742b1969f6c9 114 if(fn2_go)
IngmarLoohuis 7:742b1969f6c9 115 {
IngmarLoohuis 7:742b1969f6c9 116 fn2_go = false;
IngmarLoohuis 8:7cc4d6d9c2b5 117 motor1_Controller(radians);
IngmarLoohuis 10:54b66bd1db20 118
IngmarLoohuis 7:742b1969f6c9 119 }
IngmarLoohuis 7:742b1969f6c9 120 if(fn3_go)
IngmarLoohuis 7:742b1969f6c9 121 {
IngmarLoohuis 7:742b1969f6c9 122 fn3_go = false;
IngmarLoohuis 7:742b1969f6c9 123 getAngPosition();
IngmarLoohuis 7:742b1969f6c9 124 }
IngmarLoohuis 7:742b1969f6c9 125 }
IngmarLoohuis 2:665df4abd084 126 }