Tess Groeneveld / Motoraansturingvoortweemotorenmetbeginwaarde

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

main.cpp@1:7cafc9042056, 2013-10-25 (annotated)

Committer:
Tess
Date:
Fri Oct 25 09:22:16 2013 +0000
Revision:
1:7cafc9042056
Parent:
0:f492ec86159e
Child:
2:83dd9068b6c5
refresh from previous one

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tess 0:f492ec86159e 1 #include "mbed.h"
Tess 0:f492ec86159e 2 #include "encoder.h"
Tess 0:f492ec86159e 3 #include "MODSERIAL.h"
Tess 0:f492ec86159e 4
Tess 0:f492ec86159e 5 /*******************************************************************************
Tess 0:f492ec86159e 6 * *
Tess 0:f492ec86159e 7 * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ *
Tess 0:f492ec86159e 8 * *
Tess 0:f492ec86159e 9 ********************************************************************************/
Tess 0:f492ec86159e 10
Tess 0:f492ec86159e 11 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
Tess 0:f492ec86159e 12 void keep_in_range(float * in, float min, float max);
Tess 0:f492ec86159e 13
Tess 0:f492ec86159e 14 /** variable to show when a new loop can be started*/
Tess 0:f492ec86159e 15 /** volatile means that it can be changed in an */
Tess 0:f492ec86159e 16 /** interrupt routine, and that that change is vis-*/
Tess 0:f492ec86159e 17 /** ible in the main loop. */
Tess 0:f492ec86159e 18
Tess 0:f492ec86159e 19 volatile bool looptimerflag;
Tess 0:f492ec86159e 20
Tess 0:f492ec86159e 21 /** function called by Ticker "looptimer" */
Tess 0:f492ec86159e 22 /** variable 'looptimerflag' is set to 'true' */
Tess 0:f492ec86159e 23 /** each time the looptimer expires. */
Tess 0:f492ec86159e 24 void setlooptimerflag(void)
Tess 0:f492ec86159e 25 {
Tess 0:f492ec86159e 26 looptimerflag = true;
Tess 0:f492ec86159e 27 }
Tess 0:f492ec86159e 28
Tess 0:f492ec86159e 29 int main()
Tess 0:f492ec86159e 30 {
Tess 0:f492ec86159e 31 //LOCAL VARIABLES
Tess 0:f492ec86159e 32 /*Potmeter input*/
Tess 0:f492ec86159e 33 AnalogIn potmeterA(PTC2);
Tess 0:f492ec86159e 34 AnalogIn potmeterB(PTB2);
Tess 0:f492ec86159e 35 /* Encoder, using my encoder library */
Tess 0:f492ec86159e 36 /* First pin should be PTDx or PTAx */
Tess 0:f492ec86159e 37 /* because those pins can be used as */
Tess 0:f492ec86159e 38 /* InterruptIn */
Tess 0:f492ec86159e 39 Encoder motorA(PTD4,PTC8);
Tess 0:f492ec86159e 40 Encoder motorB(PTD0,PTD2);
Tess 0:f492ec86159e 41 /* MODSERIAL to get non-blocking Serial*/
Tess 0:f492ec86159e 42 MODSERIAL pc(USBTX,USBRX);
Tess 0:f492ec86159e 43 /* PWM control to motor */
Tess 0:f492ec86159e 44 PwmOut pwm_motorA(PTA12);
Tess 0:f492ec86159e 45 PwmOut pwm_motorB(PTA5);
Tess 0:f492ec86159e 46 /* Direction pin */
Tess 0:f492ec86159e 47 DigitalOut motordirA(PTD3);
Tess 0:f492ec86159e 48 DigitalOut motordirB(PTD1);
Tess 0:f492ec86159e 49 /* variable to store setpoint in */
Tess 0:f492ec86159e 50 float setpointA;
Tess 0:f492ec86159e 51 float setpointB;
Tess 0:f492ec86159e 52 /* variable to store pwm value in*/
Tess 0:f492ec86159e 53 float pwm_to_motorA;
Tess 0:f492ec86159e 54 float pwm_to_motorB;
Tess 1:7cafc9042056 55 int32_t positionmotorA_t0;
Tess 1:7cafc9042056 56 int32_t positionmotorB_t0;
Tess 1:7cafc9042056 57 int32_t positionmotorA_t_1;
Tess 1:7cafc9042056 58 int32_t positionmotorB_t_1;
Tess 1:7cafc9042056 59 int32_t startingpositionmotorA;
Tess 1:7cafc9042056 60 int32_t startingpositionmotorB;
Tess 1:7cafc9042056 61 int32_t velocity_motorA;
Tess 1:7cafc9042056 62 int32_t velocity_motorB;
Tess 1:7cafc9042056 63
Tess 0:f492ec86159e 64 //START OF CODE
Tess 0:f492ec86159e 65
Tess 0:f492ec86159e 66 /*Set the baudrate (use this number in RealTerm too! */
Tess 0:f492ec86159e 67 pc.baud(921600);
Tess 0:f492ec86159e 68
Tess 0:f492ec86159e 69
Tess 1:7cafc9042056 70 // in dit stukje code zorgen we ervoor dat de startpositie wordt bereikt.
Tess 0:f492ec86159e 71 motordirA.write(0);
Tess 1:7cafc9042056 72 pwm_motorA.write(.05);
Tess 1:7cafc9042056 73 positionmotorA_t0 = motorA.getPosition();
Tess 1:7cafc9042056 74 do {
Tess 1:7cafc9042056 75 wait(0.02);
Tess 1:7cafc9042056 76 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 1:7cafc9042056 77 positionmotorA_t0 = motorA.getPosition();
Tess 1:7cafc9042056 78 velocity_motorA = (positionmotorA_t0 - positionmotorA_t_1) / 0.02;
Tess 1:7cafc9042056 79 } while(velocity_motorA <= 0.01);
Tess 1:7cafc9042056 80 startingpositionmotorA = motorA.getPosition();
Tess 1:7cafc9042056 81
Tess 0:f492ec86159e 82 motordirB.write(0);
Tess 1:7cafc9042056 83 pwm_motorB.write(.05);
Tess 1:7cafc9042056 84 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 85 do {
Tess 1:7cafc9042056 86 wait(0.02);
Tess 1:7cafc9042056 87 positionmotorB_t_1 = positionmotorB_t0;
Tess 1:7cafc9042056 88 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 89 velocity_motorB = (positionmotorB_t0 - positionmotorB_t_1) / 0.02;
Tess 1:7cafc9042056 90 } while(velocity_motorB <= 0.01);
Tess 1:7cafc9042056 91 startingpositionmotorB = motorB.getPosition();
Tess 1:7cafc9042056 92
Tess 1:7cafc9042056 93
Tess 0:f492ec86159e 94 /*Create a ticker, and let it call the */
Tess 0:f492ec86159e 95 /*function 'setlooptimerflag' every 0.01s */
Tess 0:f492ec86159e 96 Ticker looptimer;
Tess 0:f492ec86159e 97 looptimer.attach(setlooptimerflag,0.01);
Tess 0:f492ec86159e 98
Tess 1:7cafc9042056 99 //INFINITE LOOP
Tess 0:f492ec86159e 100 while(1) {
Tess 0:f492ec86159e 101 /* Wait until looptimer flag is true. */
Tess 0:f492ec86159e 102 /* '!=' means not-is-equal */
Tess 0:f492ec86159e 103 while(looptimerflag != true);
Tess 0:f492ec86159e 104 /* Clear the looptimerflag, otherwise */
Tess 0:f492ec86159e 105 /* the program would simply continue */
Tess 0:f492ec86159e 106 /* without waitingin the next iteration*/
Tess 0:f492ec86159e 107 looptimerflag = false;
Tess 0:f492ec86159e 108
Tess 0:f492ec86159e 109 /* Read potmeter value, apply some math */
Tess 0:f492ec86159e 110 /* to get useful setpoint value */
Tess 0:f492ec86159e 111 setpointA = (potmeterA.read()-0.5)*(631/2);
Tess 0:f492ec86159e 112 setpointB = (potmeterB.read()-0.5)*(871/2); //1000 dan rotatie langzamer maken als lager maakt.
Tess 0:f492ec86159e 113
Tess 0:f492ec86159e 114 /* Print setpoint and current position to serial terminal*/
Tess 0:f492ec86159e 115 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 0:f492ec86159e 116 pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
Tess 0:f492ec86159e 117
Tess 0:f492ec86159e 118 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 0:f492ec86159e 119 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 0:f492ec86159e 120 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 0:f492ec86159e 121 /* Coerce pwm value if outside range */
Tess 0:f492ec86159e 122 /* Not strictly needed here, but useful */
Tess 0:f492ec86159e 123 /* if doing other calculations with pwm value */
Tess 0:f492ec86159e 124 keep_in_range(&pwm_to_motorA, -1,1);
Tess 0:f492ec86159e 125 keep_in_range(&pwm_to_motorB, -1,1);
Tess 0:f492ec86159e 126
Tess 0:f492ec86159e 127 /* Control the motor direction pin. based on */
Tess 0:f492ec86159e 128 /* the sign of your pwm value. If your */
Tess 0:f492ec86159e 129 /* motor keeps spinning when running this code */
Tess 0:f492ec86159e 130 /* you probably need to swap the motor wires, */
Tess 0:f492ec86159e 131 /* or swap the 'write(1)' and 'write(0)' below */
Tess 0:f492ec86159e 132 if(pwm_to_motorA > 0)
Tess 0:f492ec86159e 133 motordirA.write(1);
Tess 0:f492ec86159e 134 else
Tess 0:f492ec86159e 135 motordirA.write(0);
Tess 0:f492ec86159e 136 if(pwm_to_motorB > 0)
Tess 0:f492ec86159e 137 motordirB.write(1);
Tess 0:f492ec86159e 138 else
Tess 0:f492ec86159e 139 motordirB.write(0);
Tess 0:f492ec86159e 140
Tess 0:f492ec86159e 141
Tess 0:f492ec86159e 142 //WRITE VALUE TO MOTOR
Tess 0:f492ec86159e 143 /* Take the absolute value of the PWM to send */
Tess 0:f492ec86159e 144 /* to the motor. */
Tess 0:f492ec86159e 145 pwm_motorA.write(abs(pwm_to_motorA));
Tess 0:f492ec86159e 146 pwm_motorB.write(abs(pwm_to_motorB));
Tess 0:f492ec86159e 147 }
Tess 0:f492ec86159e 148 }
Tess 0:f492ec86159e 149
Tess 0:f492ec86159e 150
Tess 0:f492ec86159e 151 //coerces value 'in' to min or max when exceeding those values
Tess 0:f492ec86159e 152 //if you'd like to understand the statement below take a google for
Tess 0:f492ec86159e 153 //'ternary operators'.
Tess 0:f492ec86159e 154 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 155 {
Tess 0:f492ec86159e 156 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 157 }
Tess 0:f492ec86159e 158
Tess 0:f492ec86159e 159
Tess 0:f492ec86159e 160