Gerard Essink / Mbed 2 deprecated K9motoraansturing_copy

voor thomas

Dependencies:   BMT-K9-Regelaar Encoder MODSERIAL mbed

Fork of BMT-K9-Regelaar by First Last

main.cpp@5:19687a179088, 2013-10-25 (annotated)

Committer:
gerard1993
Date:
Fri Oct 25 11:13:01 2013 +0000
Revision:
5:19687a179088
Parent:
4:9ecf57487c72 
Versie die nog niet werkt :(

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 5:19687a179088 27
gerard1993 5:19687a179088 28 pwm_motor1.period(1.0/22000.0);
gerard1993 5:19687a179088 29
gerard1993 4:9ecf57487c72 30 //MOTOR A
vsluiter 0:7bc93f851767 31 float setpoint;
gerard1993 4:9ecf57487c72 32 float pwm_to_motor1;
gerard1993 2:3d4a843be2a9 33 float setspeed;
gerard1993 5:19687a179088 34 // float speed;
gerard1993 5:19687a179088 35 // float position2;
gerard1993 5:19687a179088 36 // float setpoint2;
gerard1993 4:9ecf57487c72 37 //MOTOR B
gerard1993 4:9ecf57487c72 38 float setpointB;
gerard1993 4:9ecf57487c72 39 float pwm_to_motor2;
gerard1993 4:9ecf57487c72 40 float setspeedB;
gerard1993 4:9ecf57487c72 41 float speedB;
gerard1993 4:9ecf57487c72 42 float position2B;
gerard1993 4:9ecf57487c72 43 float setpoint2B;
gerard1993 4:9ecf57487c72 44
vsluiter 1:9d05c0236c7e 45 //START OF CODE
gerard1993 5:19687a179088 46 pc.baud(115200);
vsluiter 0:7bc93f851767 47 Ticker looptimer;
gerard1993 5:19687a179088 48 looptimer.attach(setlooptimerflag,0.001);
gerard1993 4:9ecf57487c72 49 //A
gerard1993 5:19687a179088 50 // speed = 0;
gerard1993 5:19687a179088 51 // position2 = 0;
gerard1993 5:19687a179088 52 // setpoint = 0;
gerard1993 4:9ecf57487c72 53 //B
gerard1993 4:9ecf57487c72 54 speedB = 0;
gerard1993 4:9ecf57487c72 55 position2B = 0;
gerard1993 4:9ecf57487c72 56 setpoint2B = 0;
vsluiter 1:9d05c0236c7e 57 //INFINITE LOOP
vsluiter 0:7bc93f851767 58 while(1) {
vsluiter 1:9d05c0236c7e 59 while(looptimerflag != true);
vsluiter 0:7bc93f851767 60 looptimerflag = false;
gerard1993 4:9ecf57487c72 61
gerard1993 4:9ecf57487c72 62 //MOTOR A
gerard1993 5:19687a179088 63 setspeed = (potmeter.read()-0.5)*.0001;
gerard1993 5:19687a179088 64 setpoint = setpoint + setspeed;
gerard1993 5:19687a179088 65 pwm_to_motor1 = (setpoint - (motor1.getPosition()/4128))*20 + (setspeed - (motor1.getSpeed()/4128))*1.4 ;
gerard1993 4:9ecf57487c72 66 keep_in_range(&pwm_to_motor1, -1,1);
gerard1993 5:19687a179088 67
gerard1993 4:9ecf57487c72 68 if(pwm_to_motor1 > 0)
gerard1993 4:9ecf57487c72 69 motordir1 = 1;
vsluiter 0:7bc93f851767 70 else
gerard1993 4:9ecf57487c72 71 motordir1 = 0;
vsluiter 1:9d05c0236c7e 72 //WRITE VALUE TO MOTOR
gerard1993 4:9ecf57487c72 73 pwm_motor1.write(abs(pwm_to_motor1));
gerard1993 4:9ecf57487c72 74
gerard1993 4:9ecf57487c72 75 //MOTOR B
gerard1993 4:9ecf57487c72 76 setpointB = (potmeter.read()-0.5)*8000;
gerard1993 4:9ecf57487c72 77 setspeedB =(setpointB - setpoint2B)/0.01;
gerard1993 5:19687a179088 78 speedB = (motor2.getPosition() - position2B)/0.001;
gerard1993 4:9ecf57487c72 79 pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition());
gerard1993 4:9ecf57487c72 80 pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ;
gerard1993 4:9ecf57487c72 81 keep_in_range(&pwm_to_motor2, -1,1);
gerard1993 4:9ecf57487c72 82 setpoint2B = setpointB;
gerard1993 4:9ecf57487c72 83 position2B = motor2.getPosition();
gerard1993 4:9ecf57487c72 84 if(pwm_to_motor2 > 0)
gerard1993 4:9ecf57487c72 85 motordir2 = 1;
gerard1993 4:9ecf57487c72 86 else
gerard1993 4:9ecf57487c72 87 motordir2 = 0;
gerard1993 4:9ecf57487c72 88 //WRITE VALUE TO MOTOR
gerard1993 4:9ecf57487c72 89 pwm_motor2.write(abs(pwm_to_motor2));
vsluiter 0:7bc93f851767 90 }
vsluiter 0:7bc93f851767 91 }
vsluiter 0:7bc93f851767 92
vsluiter 0:7bc93f851767 93
vsluiter 0:7bc93f851767 94 //coerces value 'in' to min or max when exceeding those values
vsluiter 0:7bc93f851767 95 //if you'd like to understand the statement below take a google for
vsluiter 0:7bc93f851767 96 //'ternary operators'.
vsluiter 1:9d05c0236c7e 97 void keep_in_range(float * in, float min, float max)
vsluiter 0:7bc93f851767 98 {
vsluiter 0:7bc93f851767 99 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:7bc93f851767 100 }
vsluiter 0:7bc93f851767 101
vsluiter 0:7bc93f851767 102
vsluiter 0:7bc93f851767 103