Ramon Waninge / Mbed 2 deprecated Milestone1

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:   FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@23:0c02cf961344, 2018-10-29 (annotated)

Committer:
efvanmarrewijk
Date:
Mon Oct 29 12:31:55 2018 +0000
Revision:
23:0c02cf961344
Parent:
22:71524e4fd1f2 
Not working, but with several additional functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
efvanmarrewijk 16:720365110953 1 // Inclusion of libraries
Ramonwaninge 0:3ea1bbfbeaae 2 #include "mbed.h"
efvanmarrewijk 11:3efd6a324f16 3 #include "FastPWM.h"
efvanmarrewijk 11:3efd6a324f16 4 #include "QEI.h" // Includes library for encoder
efvanmarrewijk 13:6556cd086d07 5 #include "MODSERIAL.h"
efvanmarrewijk 13:6556cd086d07 6 #include "HIDScope.h"
efvanmarrewijk 13:6556cd086d07 7 #include "BiQuad.h"
Ramonwaninge 0:3ea1bbfbeaae 8
efvanmarrewijk 14:e21cb701ccb8 9 // Input
efvanmarrewijk 21:363271dcfe1f 10 AnalogIn pot1(A1);
efvanmarrewijk 21:363271dcfe1f 11 AnalogIn pot2(A2);
efvanmarrewijk 21:363271dcfe1f 12 InterruptIn button1(D0);
efvanmarrewijk 21:363271dcfe1f 13 InterruptIn button2(D1);
efvanmarrewijk 21:363271dcfe1f 14 InterruptIn emergencybutton(SW2); /* This is not yet implemented!
efvanmarrewijk 21:363271dcfe1f 15 The button SW2 on the K64F is the emergency button: if you press this,
efvanmarrewijk 21:363271dcfe1f 16 everything will abort as soon as possible
efvanmarrewijk 21:363271dcfe1f 17 */
efvanmarrewijk 21:363271dcfe1f 18
efvanmarrewijk 21:363271dcfe1f 19 DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk 21:363271dcfe1f 20 DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk 21:363271dcfe1f 21 DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk 21:363271dcfe1f 22 DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk 21:363271dcfe1f 23 DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk 21:363271dcfe1f 24 DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk 9:65c52c1f4a57 25
efvanmarrewijk 14:e21cb701ccb8 26 // Output
efvanmarrewijk 23:0c02cf961344 27 DigitalOut pin2(D2); // Motor 3 direction
efvanmarrewijk 23:0c02cf961344 28 FastPWM pin3(D3); // Motor 3 pwm
efvanmarrewijk 23:0c02cf961344 29 DigitalOut pin4(D4); // Motor 2 direction
efvanmarrewijk 23:0c02cf961344 30 FastPWM pin5(D5); // Motor 2 pwm
efvanmarrewijk 23:0c02cf961344 31 FastPWM pin6(D6); // Motor 1 pwm
efvanmarrewijk 23:0c02cf961344 32 DigitalOut pin7(D7); // Motor 1 direction
efvanmarrewijk 21:363271dcfe1f 33 //float u1 = pot1;
Ramonwaninge 2:d8a552d1d33a 34
efvanmarrewijk 16:720365110953 35 // Utilisation of libraries
efvanmarrewijk 16:720365110953 36 MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk 18:ca084c362855 37 QEI Encoder1(D11,D10,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 18:ca084c362855 38 QEI Encoder2(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 18:ca084c362855 39 QEI Encoder3(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk 20:695140b8db2f 40 Ticker motor;
efvanmarrewijk 23:0c02cf961344 41 Ticker encoder;
efvanmarrewijk 9:65c52c1f4a57 42
efvanmarrewijk 16:720365110953 43 // Global variables
efvanmarrewijk 23:0c02cf961344 44 const float pi = 3.14159265358979;
efvanmarrewijk 23:0c02cf961344 45 const double dCountsRad = 4200.0;
efvanmarrewijk 23:0c02cf961344 46 const float fCountsRad = 4200.0;
efvanmarrewijk 22:71524e4fd1f2 47 double u3 = 0.0; // Normalised variable for the movement of motor 3
efvanmarrewijk 16:720365110953 48
efvanmarrewijk 23:0c02cf961344 49
efvanmarrewijk 16:720365110953 50 // Functions
efvanmarrewijk 23:0c02cf961344 51 /*float AngleCalc(float counts)
efvanmarrewijk 23:0c02cf961344 52 { float angle = ((float)counts*2.0*pi)/fCountsRad;
efvanmarrewijk 23:0c02cf961344 53 /*if (angle > 2.0*pi)
efvanmarrewijk 23:0c02cf961344 54 { angle = angle - 2.0*pi;
efvanmarrewijk 23:0c02cf961344 55 }
efvanmarrewijk 23:0c02cf961344 56 else if (angle < 2.0*pi)
efvanmarrewijk 23:0c02cf961344 57 { angle = angle + 2.0*pi;
efvanmarrewijk 23:0c02cf961344 58 }
efvanmarrewijk 23:0c02cf961344 59
efvanmarrewijk 23:0c02cf961344 60 return angle;
efvanmarrewijk 23:0c02cf961344 61 }
efvanmarrewijk 23:0c02cf961344 62 */
efvanmarrewijk 23:0c02cf961344 63
efvanmarrewijk 18:ca084c362855 64 void Encoderinput()
efvanmarrewijk 18:ca084c362855 65 { int counts1;
efvanmarrewijk 18:ca084c362855 66 int counts2;
efvanmarrewijk 18:ca084c362855 67 int counts3;
efvanmarrewijk 18:ca084c362855 68 float angle1;
efvanmarrewijk 18:ca084c362855 69 float angle2;
efvanmarrewijk 18:ca084c362855 70 float angle3;
efvanmarrewijk 18:ca084c362855 71 counts1 = Encoder1.getPulses();
efvanmarrewijk 18:ca084c362855 72 counts2 = Encoder2.getPulses();
efvanmarrewijk 18:ca084c362855 73 counts3 = Encoder3.getPulses();
efvanmarrewijk 23:0c02cf961344 74 //angle1 = AngleCalc (counts1);
efvanmarrewijk 23:0c02cf961344 75 //angle2 = AngleCalc (counts2);
efvanmarrewijk 23:0c02cf961344 76 //angle3 = AngleCalc (counts3);
efvanmarrewijk 23:0c02cf961344 77 angle1 = ((float)counts1*2.0*pi)/fCountsRad;
efvanmarrewijk 23:0c02cf961344 78 angle2 = ((float)counts2*2.0*pi)/fCountsRad;
efvanmarrewijk 23:0c02cf961344 79
efvanmarrewijk 23:0c02cf961344 80 pc.printf("Counts1,2,3: %i %i %i Angle1,2,3: %f %f %f \r\n",counts1,counts2,counts3,angle1,angle2,angle3);
efvanmarrewijk 18:ca084c362855 81 }
efvanmarrewijk 16:720365110953 82
efvanmarrewijk 23:0c02cf961344 83 /*double RefVelocity()
efvanmarrewijk 23:0c02cf961344 84 { // Returns reference velocity in rad/s.
efvanmarrewijk 23:0c02cf961344 85 // Positive value means clockwise rotation.
efvanmarrewijk 23:0c02cf961344 86 const float maxVelocity=8.4; // in rad/s of course!
efvanmarrewijk 23:0c02cf961344 87 double RefVelocity; // in rad/s
efvanmarrewijk 14:e21cb701ccb8 88
efvanmarrewijk 23:0c02cf961344 89 if (button1 == 1)
efvanmarrewijk 23:0c02cf961344 90 { // Clockwise rotation
efvanmarrewijk 23:0c02cf961344 91 RefVelocity = potMeterIn * maxVelocity;
efvanmarrewijk 23:0c02cf961344 92 }
efvanmarrewijk 23:0c02cf961344 93 else
efvanmarrewijk 23:0c02cf961344 94 { // Counterclockwise rotation
efvanmarrewijk 23:0c02cf961344 95 RefVelocity = -1*potMeterIn * maxVelocity;
efvanmarrewijk 23:0c02cf961344 96 }
efvanmarrewijk 23:0c02cf961344 97 return RefVelocity;
efvanmarrewijk 21:363271dcfe1f 98 }
efvanmarrewijk 23:0c02cf961344 99
efvanmarrewijk 23:0c02cf961344 100 double ActualPosition()
efvanmarrewijk 23:0c02cf961344 101 {
efvanmarrewijk 23:0c02cf961344 102 double MotorPos = - (counts1 - offsetcounts1) / dCountsRad;
efvanmarrewijk 23:0c02cf961344 103 // minus sign to correct for direction convention
efvanmarrewijk 23:0c02cf961344 104 return MotorPos;
efvanmarrewijk 23:0c02cf961344 105 }
efvanmarrewijk 23:0c02cf961344 106 */
efvanmarrewijk 14:e21cb701ccb8 107
efvanmarrewijk 23:0c02cf961344 108 void turn()
efvanmarrewijk 11:3efd6a324f16 109 /* Function for the movement of all motors, using the potmeters for the moving
efvanmarrewijk 11:3efd6a324f16 110 direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
efvanmarrewijk 11:3efd6a324f16 111 shield for the moving direction and speed of motor 3.
efvanmarrewijk 11:3efd6a324f16 112 */
efvanmarrewijk 11:3efd6a324f16 113 {
efvanmarrewijk 23:0c02cf961344 114 double u1 = 2.0*(pot1 - 0.5); // Normalised variable for the movement of motor 2
efvanmarrewijk 6:3c9569087274 115 if (u1>0)
efvanmarrewijk 6:3c9569087274 116 { pin4 = true;
efvanmarrewijk 6:3c9569087274 117 }
efvanmarrewijk 6:3c9569087274 118 else if(u1<0)
efvanmarrewijk 6:3c9569087274 119 { pin4 = false;
efvanmarrewijk 6:3c9569087274 120 }
efvanmarrewijk 23:0c02cf961344 121 pin5.period(fabs(u1)); // Set PWM period to 0.2 seconds
efvanmarrewijk 23:0c02cf961344 122 //pin5 = fabs(u1);
efvanmarrewijk 23:0c02cf961344 123 //float a1=fabs(u1);
efvanmarrewijk 23:0c02cf961344 124 //pin5.write(a1); // Set duty cycle (and thus the angle of the motor) to potmeter percentage
efvanmarrewijk 6:3c9569087274 125
efvanmarrewijk 23:0c02cf961344 126 double u2 = 2.0*(pot2 - 0.5); // Normalised variable for the movement of motor 1
efvanmarrewijk 11:3efd6a324f16 127 if (u2<0)
efvanmarrewijk 6:3c9569087274 128 { pin7 = true;
efvanmarrewijk 6:3c9569087274 129 }
efvanmarrewijk 11:3efd6a324f16 130 else if(u2>0)
efvanmarrewijk 6:3c9569087274 131 { pin7 = false;
efvanmarrewijk 6:3c9569087274 132 }
efvanmarrewijk 23:0c02cf961344 133 pin6.period(fabs(u2)); // Set PWM period to 0.2 seconds
efvanmarrewijk 23:0c02cf961344 134 //float a2 = fabs(u2);
efvanmarrewijk 23:0c02cf961344 135 //pin6.write(a2); // Set output duty cycle (and thus the angle of the motor) to potmeter percentage
Ramonwaninge 3:d39285fdd103 136 }
efvanmarrewijk 11:3efd6a324f16 137
efvanmarrewijk 16:720365110953 138 // Main program
efvanmarrewijk 11:3efd6a324f16 139 int main()
efvanmarrewijk 18:ca084c362855 140 {
efvanmarrewijk 14:e21cb701ccb8 141 pc.baud(115200);
efvanmarrewijk 23:0c02cf961344 142
efvanmarrewijk 23:0c02cf961344 143 motor.attach(turn, 0.001);
efvanmarrewijk 23:0c02cf961344 144 //encoder.attach(Encoderinput,0.01);
efvanmarrewijk 22:71524e4fd1f2 145
efvanmarrewijk 16:720365110953 146 while (true)
efvanmarrewijk 23:0c02cf961344 147 { //
efvanmarrewijk 23:0c02cf961344 148 Encoderinput();
Ramonwaninge 3:d39285fdd103 149 }
Ramonwaninge 0:3ea1bbfbeaae 150 }