Astrid Schut / Mbed 2 deprecated PWMmotor

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

main.cpp@12:39ba69bb5a03, 2019-04-04 (annotated)

Committer:
aschut
Date:
Thu Apr 04 11:11:23 2019 +0000
Revision:
12:39ba69bb5a03
Parent:
5:4b25551aeb6e 
motor 1 check

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:a9a42914138c 1 #include "mbed.h"
aschut 2:926d56babb1a 2 #include "MODSERIAL.h"
aschut 4:99f7fdce608e 3 #include "QEI.h"
aschut 4:99f7fdce608e 4
aschut 4:99f7fdce608e 5 // Algemeen
aschut 1:58f34947c674 6 DigitalIn button2(SW2);
aschut 1:58f34947c674 7 DigitalIn button3(SW3);
aschut 2:926d56babb1a 8 DigitalOut led(LED_GREEN);
aschut 2:926d56babb1a 9 DigitalOut led2(LED_RED);
aschut 4:99f7fdce608e 10 DigitalOut led3(LED_BLUE);
aschut 2:926d56babb1a 11 MODSERIAL pc(USBTX, USBRX);
aschut 2:926d56babb1a 12
aschut 4:99f7fdce608e 13 //Motoren
aschut 12:39ba69bb5a03 14 DigitalOut direction1(D7);
aschut 12:39ba69bb5a03 15 PwmOut pwmpin1(D6);
aschut 12:39ba69bb5a03 16 PwmOut pwmpin2(D5);
aschut 12:39ba69bb5a03 17 DigitalOut direction2(D4);
aschut 4:99f7fdce608e 18 volatile float PWM1;
aschut 4:99f7fdce608e 19 volatile float PWM2;
aschut 4:99f7fdce608e 20 volatile float pwm2;
aschut 4:99f7fdce608e 21
aschut 4:99f7fdce608e 22 //Encoder
aschut 4:99f7fdce608e 23 DigitalIn EncoderA(D13);
aschut 4:99f7fdce608e 24 DigitalIn EncoderB(D12);
aschut 12:39ba69bb5a03 25 QEI encoder2 (D13, D12, NC, 1200, QEI::X4_ENCODING);
aschut 4:99f7fdce608e 26 float Pulses2;
aschut 4:99f7fdce608e 27 float Degrees2;
aschut 2:926d56babb1a 28
aschut 2:926d56babb1a 29 //Pot meter
aschut 1:58f34947c674 30 AnalogIn pot(A1);
aschut 4:99f7fdce608e 31 float Pot2;
aschut 4:99f7fdce608e 32
aschut 4:99f7fdce608e 33 //Ticker
aschut 4:99f7fdce608e 34 Ticker Pwm;
aschut 2:926d56babb1a 35 Ticker PotRead;
aschut 5:4b25551aeb6e 36 Ticker Kin;
aschut 0:a9a42914138c 37
aschut 5:4b25551aeb6e 38 //Kinematica
aschut 5:4b25551aeb6e 39 float stap;
aschut 5:4b25551aeb6e 40 float KPot;
aschut 5:4b25551aeb6e 41 float KPotabs;
aschut 5:4b25551aeb6e 42 float ElbowReference;
aschut 5:4b25551aeb6e 43 float Ellebooghoek1;
aschut 5:4b25551aeb6e 44 float Ellebooghoek2;
aschut 5:4b25551aeb6e 45 float Ellebooghoek3;
aschut 5:4b25551aeb6e 46 float Ellebooghoek4;
aschut 5:4b25551aeb6e 47 float Hoeknieuw;
aschut 5:4b25551aeb6e 48
aschut 5:4b25551aeb6e 49 //Limiet in graden
aschut 5:4b25551aeb6e 50 float lowerlim = 0;
aschut 5:4b25551aeb6e 51 float upperlim = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
aschut 2:926d56babb1a 52
aschut 5:4b25551aeb6e 53
aschut 5:4b25551aeb6e 54 float Kinematics(float KPot)
aschut 5:4b25551aeb6e 55 {
aschut 5:4b25551aeb6e 56
aschut 5:4b25551aeb6e 57 if (KPot > 0.45f){
aschut 5:4b25551aeb6e 58 stap = KPot*15; // 144 graden van de arm in 5 seconden
aschut 5:4b25551aeb6e 59 Hoeknieuw = ElbowReference + stap;
aschut 5:4b25551aeb6e 60 return Hoeknieuw;
aschut 5:4b25551aeb6e 61 }
aschut 5:4b25551aeb6e 62
aschut 5:4b25551aeb6e 63 else if (KPot < -0.45f){
aschut 5:4b25551aeb6e 64 stap = KPot*15;
aschut 5:4b25551aeb6e 65 Hoeknieuw = ElbowReference + stap;
aschut 5:4b25551aeb6e 66 return Hoeknieuw;
aschut 5:4b25551aeb6e 67 }
aschut 5:4b25551aeb6e 68
aschut 5:4b25551aeb6e 69 else{
aschut 5:4b25551aeb6e 70 return ElbowReference;
aschut 5:4b25551aeb6e 71 }
aschut 5:4b25551aeb6e 72 }
aschut 4:99f7fdce608e 73
aschut 5:4b25551aeb6e 74 float Limits(float Ellebooghoek2){
aschut 5:4b25551aeb6e 75
aschut 5:4b25551aeb6e 76 if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) { //Binnen de limieten
aschut 5:4b25551aeb6e 77 Ellebooghoek3 = Ellebooghoek2;
aschut 5:4b25551aeb6e 78 }
aschut 5:4b25551aeb6e 79
aschut 5:4b25551aeb6e 80 else {
aschut 5:4b25551aeb6e 81 if (Ellebooghoek2 >= upperlim) { //Boven de limiet
aschut 5:4b25551aeb6e 82 Ellebooghoek3 = upperlim;
aschut 5:4b25551aeb6e 83 }
aschut 5:4b25551aeb6e 84 else { //Onder de limiet
aschut 5:4b25551aeb6e 85 Ellebooghoek3 = lowerlim;
aschut 5:4b25551aeb6e 86 }
aschut 5:4b25551aeb6e 87 }
aschut 5:4b25551aeb6e 88
aschut 5:4b25551aeb6e 89 return Ellebooghoek3;
aschut 5:4b25551aeb6e 90 }
aschut 4:99f7fdce608e 91
aschut 0:a9a42914138c 92
aschut 1:58f34947c674 93 void Period(void)
aschut 1:58f34947c674 94 {
aschut 1:58f34947c674 95 pwmpin2.period_us(60);
aschut 0:a9a42914138c 96 }
aschut 4:99f7fdce608e 97
aschut 4:99f7fdce608e 98 void PwmMotor(void)
aschut 1:58f34947c674 99 {
aschut 5:4b25551aeb6e 100 float Ellebooghoek1 = Kinematics(pwm2);
aschut 5:4b25551aeb6e 101 float Ellebooghoek4 = Limits(Ellebooghoek1);
aschut 5:4b25551aeb6e 102 ElbowReference = Ellebooghoek4;
aschut 5:4b25551aeb6e 103
aschut 5:4b25551aeb6e 104 pc.printf("ElbowReference:%f\n", ElbowReference);
aschut 3:ac13255164cd 105 pc.printf("pwm2=%f\r\n",pwm2);
aschut 5:4b25551aeb6e 106
aschut 12:39ba69bb5a03 107 Degrees2 = (Pulses2/1200)*360;
aschut 5:4b25551aeb6e 108 pc.printf("Degrees is:%f\n", Degrees2);
aschut 3:ac13255164cd 109 direction2 = pwm2 < 0.0f; //positief = CW, negatief = CCW
aschut 3:ac13255164cd 110 pwmpin2 = fabs(pwm2);
aschut 4:99f7fdce608e 111
aschut 2:926d56babb1a 112
aschut 1:58f34947c674 113 }
aschut 1:58f34947c674 114 void MotorOn(void)
aschut 1:58f34947c674 115 {
aschut 4:99f7fdce608e 116 pwmpin2 = 0;
aschut 4:99f7fdce608e 117 Pwm.attach (PwmMotor, 0.1);
aschut 5:4b25551aeb6e 118
aschut 1:58f34947c674 119 }
aschut 1:58f34947c674 120 void MotorOff(void)
aschut 1:58f34947c674 121 {
aschut 4:99f7fdce608e 122 Pwm.detach ();
aschut 1:58f34947c674 123 pwmpin2 = 0;
aschut 1:58f34947c674 124 }
aschut 4:99f7fdce608e 125
aschut 4:99f7fdce608e 126 void ContinuousReader(void){
aschut 3:ac13255164cd 127 Pot2 = pot.read();
aschut 5:4b25551aeb6e 128 pwm2 =(Pot2*2)-1; //scaling
aschut 4:99f7fdce608e 129 Pulses2 = encoder2.getPulses();
aschut 12:39ba69bb5a03 130 pc.printf("Pulses is:%f\n", Pulses2);
aschut 12:39ba69bb5a03 131 wait(0.001);
aschut 4:99f7fdce608e 132 //pc.printf("%f\r\n",Pot2);
aschut 2:926d56babb1a 133 }
aschut 1:58f34947c674 134
aschut 1:58f34947c674 135
aschut 1:58f34947c674 136
aschut 0:a9a42914138c 137 int main() {
aschut 0:a9a42914138c 138 Period();
aschut 4:99f7fdce608e 139 PotRead.attach(ContinuousReader,0.1);
aschut 2:926d56babb1a 140 pc.baud(115200);
aschut 2:926d56babb1a 141 pc.printf("start\r\n");
aschut 4:99f7fdce608e 142 led = 1;
aschut 4:99f7fdce608e 143 led2 =1;
aschut 4:99f7fdce608e 144 led3 =1;
aschut 2:926d56babb1a 145
aschut 2:926d56babb1a 146 while (true){
aschut 4:99f7fdce608e 147 led3 = 0;
aschut 1:58f34947c674 148 if (!button2)
aschut 1:58f34947c674 149 {
aschut 4:99f7fdce608e 150 led3 = 1;
aschut 4:99f7fdce608e 151 led = 0;
aschut 4:99f7fdce608e 152 pc.printf("MotorOn\r\n");
aschut 2:926d56babb1a 153 MotorOn();
aschut 1:58f34947c674 154 }
aschut 1:58f34947c674 155 if (!button3)
aschut 1:58f34947c674 156 {
aschut 4:99f7fdce608e 157 pc.printf("MotorOff\r\n");
aschut 4:99f7fdce608e 158 PotRead.detach();
aschut 1:58f34947c674 159 MotorOff();
aschut 1:58f34947c674 160 }
aschut 2:926d56babb1a 161 led = 0;
aschut 3:ac13255164cd 162 }
aschut 0:a9a42914138c 163 }
aschut 0:a9a42914138c 164
aschut 0:a9a42914138c 165
aschut 0:a9a42914138c 166