Gerard Essink / Mbed 2 deprecated K9motoraansturing_copy

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:   BMT-K9-Regelaar Encoder MODSERIAL mbed

Fork of BMT-K9-Regelaar by First Last

main.cpp@4:9ecf57487c72, 2013-10-18 (annotated)

Committer:
gerard1993
Date:
Fri Oct 18 09:34:16 2013 +0000
Revision:
4:9ecf57487c72
Parent:
3:1241d75b7f49
Child:
5:19687a179088
voor thomas

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:7bc93f851767 1 #include "mbed.h"
vsluiter 0:7bc93f851767 2 #include "encoder.h"
vsluiter 0:7bc93f851767 3 #include "MODSERIAL.h"
vsluiter 0:7bc93f851767 4
vsluiter 1:9d05c0236c7e 5 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
vsluiter 1:9d05c0236c7e 6 void keep_in_range(float * in, float min, float max);
vsluiter 1:9d05c0236c7e 7
vsluiter 0:7bc93f851767 8 volatile bool looptimerflag;
vsluiter 0:7bc93f851767 9
vsluiter 0:7bc93f851767 10 void setlooptimerflag(void)
vsluiter 0:7bc93f851767 11 {
vsluiter 0:7bc93f851767 12 looptimerflag = true;
vsluiter 0:7bc93f851767 13 }
vsluiter 0:7bc93f851767 14
vsluiter 0:7bc93f851767 15
vsluiter 0:7bc93f851767 16 int main() {
vsluiter 1:9d05c0236c7e 17 //LOCAL VARIABLES
vsluiter 1:9d05c0236c7e 18 AnalogIn potmeter(PTC2);
vsluiter 0:7bc93f851767 19 MODSERIAL pc(USBTX,USBRX);
gerard1993 4:9ecf57487c72 20
gerard1993 4:9ecf57487c72 21 Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx
gerard1993 4:9ecf57487c72 22 Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx
gerard1993 4:9ecf57487c72 23 PwmOut pwm_motor1(PTA12);
gerard1993 4:9ecf57487c72 24 PwmOut pwm_motor2(PTA5);
gerard1993 4:9ecf57487c72 25 DigitalOut motordir1(PTD3);
gerard1993 4:9ecf57487c72 26 DigitalOut motordir2(PTD1);
gerard1993 4:9ecf57487c72 27 //MOTOR A
vsluiter 0:7bc93f851767 28 float setpoint;
gerard1993 4:9ecf57487c72 29 float pwm_to_motor1;
gerard1993 2:3d4a843be2a9 30 float setspeed;
gerard1993 2:3d4a843be2a9 31 float speed;
gerard1993 2:3d4a843be2a9 32 float position2;
gerard1993 2:3d4a843be2a9 33 float setpoint2;
gerard1993 4:9ecf57487c72 34 //MOTOR B
gerard1993 4:9ecf57487c72 35 float setpointB;
gerard1993 4:9ecf57487c72 36 float pwm_to_motor2;
gerard1993 4:9ecf57487c72 37 float setspeedB;
gerard1993 4:9ecf57487c72 38 float speedB;
gerard1993 4:9ecf57487c72 39 float position2B;
gerard1993 4:9ecf57487c72 40 float setpoint2B;
gerard1993 4:9ecf57487c72 41
vsluiter 1:9d05c0236c7e 42 //START OF CODE
vsluiter 1:9d05c0236c7e 43 pc.baud(230400);
vsluiter 0:7bc93f851767 44 Ticker looptimer;
vsluiter 0:7bc93f851767 45 looptimer.attach(setlooptimerflag,0.01);
vsluiter 1:9d05c0236c7e 46 pc.printf("bla");
gerard1993 4:9ecf57487c72 47 //A
gerard1993 2:3d4a843be2a9 48 speed = 0;
gerard1993 2:3d4a843be2a9 49 position2 = 0;
gerard1993 2:3d4a843be2a9 50 setpoint2 = 0;
gerard1993 4:9ecf57487c72 51 //B
gerard1993 4:9ecf57487c72 52 speedB = 0;
gerard1993 4:9ecf57487c72 53 position2B = 0;
gerard1993 4:9ecf57487c72 54 setpoint2B = 0;
vsluiter 1:9d05c0236c7e 55 //INFINITE LOOP
vsluiter 0:7bc93f851767 56 while(1) {
vsluiter 1:9d05c0236c7e 57 while(looptimerflag != true);
vsluiter 0:7bc93f851767 58 looptimerflag = false;
gerard1993 4:9ecf57487c72 59
gerard1993 4:9ecf57487c72 60 //MOTOR A
gerard1993 4:9ecf57487c72 61 setpoint = (potmeter.read()-0.5)*8000;
gerard1993 2:3d4a843be2a9 62 setspeed =(setpoint - setpoint2)/0.01;
gerard1993 2:3d4a843be2a9 63 speed = (motor1.getPosition() - position2)/0.01;
vsluiter 0:7bc93f851767 64 pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
gerard1993 4:9ecf57487c72 65 pwm_to_motor1 = (setpoint - motor1.getPosition())*.0001 + (setspeed - speed)*.00005 ;
gerard1993 4:9ecf57487c72 66 keep_in_range(&pwm_to_motor1, -1,1);
gerard1993 2:3d4a843be2a9 67 setpoint2 = setpoint;
gerard1993 2:3d4a843be2a9 68 position2 = motor1.getPosition();
gerard1993 4:9ecf57487c72 69 if(pwm_to_motor1 > 0)
gerard1993 4:9ecf57487c72 70 motordir1 = 1;
vsluiter 0:7bc93f851767 71 else
gerard1993 4:9ecf57487c72 72 motordir1 = 0;
vsluiter 1:9d05c0236c7e 73 //WRITE VALUE TO MOTOR
gerard1993 4:9ecf57487c72 74 pwm_motor1.write(abs(pwm_to_motor1));
gerard1993 4:9ecf57487c72 75
gerard1993 4:9ecf57487c72 76 //MOTOR B
gerard1993 4:9ecf57487c72 77 setpointB = (potmeter.read()-0.5)*8000;
gerard1993 4:9ecf57487c72 78 setspeedB =(setpointB - setpoint2B)/0.01;
gerard1993 4:9ecf57487c72 79 speedB = (motor2.getPosition() - position2B)/0.01;
gerard1993 4:9ecf57487c72 80 pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition());
gerard1993 4:9ecf57487c72 81 pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ;
gerard1993 4:9ecf57487c72 82 keep_in_range(&pwm_to_motor2, -1,1);
gerard1993 4:9ecf57487c72 83 setpoint2B = setpointB;
gerard1993 4:9ecf57487c72 84 position2B = motor2.getPosition();
gerard1993 4:9ecf57487c72 85 if(pwm_to_motor2 > 0)
gerard1993 4:9ecf57487c72 86 motordir2 = 1;
gerard1993 4:9ecf57487c72 87 else
gerard1993 4:9ecf57487c72 88 motordir2 = 0;
gerard1993 4:9ecf57487c72 89 //WRITE VALUE TO MOTOR
gerard1993 4:9ecf57487c72 90 pwm_motor2.write(abs(pwm_to_motor2));
vsluiter 0:7bc93f851767 91 }
vsluiter 0:7bc93f851767 92 }
vsluiter 0:7bc93f851767 93
vsluiter 0:7bc93f851767 94
vsluiter 0:7bc93f851767 95 //coerces value 'in' to min or max when exceeding those values
vsluiter 0:7bc93f851767 96 //if you'd like to understand the statement below take a google for
vsluiter 0:7bc93f851767 97 //'ternary operators'.
vsluiter 1:9d05c0236c7e 98 void keep_in_range(float * in, float min, float max)
vsluiter 0:7bc93f851767 99 {
vsluiter 0:7bc93f851767 100 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:7bc93f851767 101 }
vsluiter 0:7bc93f851767 102
vsluiter 0:7bc93f851767 103
vsluiter 0:7bc93f851767 104