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@0:f492ec86159e, 2013-10-25 (annotated)

Committer:
Tess
Date:
Fri Oct 25 08:07:43 2013 +0000
Revision:
0:f492ec86159e
Child:
1:7cafc9042056
Heey, this is a file to make your two motors work together. Have fun

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 0:f492ec86159e 55 int32_t beginpositiemotorA_t0;
Tess 0:f492ec86159e 56 int32_t beginpositiemotorB_t0;
Tess 0:f492ec86159e 57 //START OF CODE
Tess 0:f492ec86159e 58
Tess 0:f492ec86159e 59 /*Set the baudrate (use this number in RealTerm too! */
Tess 0:f492ec86159e 60 pc.baud(921600);
Tess 0:f492ec86159e 61
Tess 0:f492ec86159e 62
Tess 0:f492ec86159e 63 // wil dat 20 seconden de motor rechtsom gaat draaien
Tess 0:f492ec86159e 64 motordirA.write(0);
Tess 0:f492ec86159e 65 motordirB.write(0);
Tess 0:f492ec86159e 66 pwm_motorA.write(.1);
Tess 0:f492ec86159e 67 pwm_motorB.write(.1);
Tess 0:f492ec86159e 68
Tess 0:f492ec86159e 69 do
Tess 0:f492ec86159e 70 {
Tess 0:f492ec86159e 71 //wacht
Tess 0:f492ec86159e 72 //schuif t0 / t1
Tess 0:f492ec86159e 73 lees encoder
Tess 0:f492ec86159e 74
Tess 0:f492ec86159e 75 }while(posititie ongelijk);
Tess 0:f492ec86159e 76
Tess 0:f492ec86159e 77
Tess 0:f492ec86159e 78
Tess 0:f492ec86159e 79
Tess 0:f492ec86159e 80
Tess 0:f492ec86159e 81 /*Create a ticker, and let it call the */
Tess 0:f492ec86159e 82 /*function 'setlooptimerflag' every 0.01s */
Tess 0:f492ec86159e 83 Ticker looptimer;
Tess 0:f492ec86159e 84 looptimer.attach(setlooptimerflag,0.01);
Tess 0:f492ec86159e 85
Tess 0:f492ec86159e 86 //INFINITE LOOP
Tess 0:f492ec86159e 87 while(1) {
Tess 0:f492ec86159e 88 /* Wait until looptimer flag is true. */
Tess 0:f492ec86159e 89 /* '!=' means not-is-equal */
Tess 0:f492ec86159e 90 while(looptimerflag != true);
Tess 0:f492ec86159e 91 /* Clear the looptimerflag, otherwise */
Tess 0:f492ec86159e 92 /* the program would simply continue */
Tess 0:f492ec86159e 93 /* without waitingin the next iteration*/
Tess 0:f492ec86159e 94 looptimerflag = false;
Tess 0:f492ec86159e 95
Tess 0:f492ec86159e 96 /* Read potmeter value, apply some math */
Tess 0:f492ec86159e 97 /* to get useful setpoint value */
Tess 0:f492ec86159e 98 setpointA = (potmeterA.read()-0.5)*(631/2);
Tess 0:f492ec86159e 99 setpointB = (potmeterB.read()-0.5)*(871/2); //1000 dan rotatie langzamer maken als lager maakt.
Tess 0:f492ec86159e 100
Tess 0:f492ec86159e 101 /* Print setpoint and current position to serial terminal*/
Tess 0:f492ec86159e 102 pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
Tess 0:f492ec86159e 103 pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
Tess 0:f492ec86159e 104
Tess 0:f492ec86159e 105 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
Tess 0:f492ec86159e 106 pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
Tess 0:f492ec86159e 107 pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
Tess 0:f492ec86159e 108 /* Coerce pwm value if outside range */
Tess 0:f492ec86159e 109 /* Not strictly needed here, but useful */
Tess 0:f492ec86159e 110 /* if doing other calculations with pwm value */
Tess 0:f492ec86159e 111 keep_in_range(&pwm_to_motorA, -1,1);
Tess 0:f492ec86159e 112 keep_in_range(&pwm_to_motorB, -1,1);
Tess 0:f492ec86159e 113
Tess 0:f492ec86159e 114 /* Control the motor direction pin. based on */
Tess 0:f492ec86159e 115 /* the sign of your pwm value. If your */
Tess 0:f492ec86159e 116 /* motor keeps spinning when running this code */
Tess 0:f492ec86159e 117 /* you probably need to swap the motor wires, */
Tess 0:f492ec86159e 118 /* or swap the 'write(1)' and 'write(0)' below */
Tess 0:f492ec86159e 119 if(pwm_to_motorA > 0)
Tess 0:f492ec86159e 120 motordirA.write(1);
Tess 0:f492ec86159e 121 else
Tess 0:f492ec86159e 122 motordirA.write(0);
Tess 0:f492ec86159e 123 if(pwm_to_motorB > 0)
Tess 0:f492ec86159e 124 motordirB.write(1);
Tess 0:f492ec86159e 125 else
Tess 0:f492ec86159e 126 motordirB.write(0);
Tess 0:f492ec86159e 127
Tess 0:f492ec86159e 128
Tess 0:f492ec86159e 129 //WRITE VALUE TO MOTOR
Tess 0:f492ec86159e 130 /* Take the absolute value of the PWM to send */
Tess 0:f492ec86159e 131 /* to the motor. */
Tess 0:f492ec86159e 132 pwm_motorA.write(abs(pwm_to_motorA));
Tess 0:f492ec86159e 133 pwm_motorB.write(abs(pwm_to_motorB));
Tess 0:f492ec86159e 134 }
Tess 0:f492ec86159e 135 }
Tess 0:f492ec86159e 136
Tess 0:f492ec86159e 137
Tess 0:f492ec86159e 138 //coerces value 'in' to min or max when exceeding those values
Tess 0:f492ec86159e 139 //if you'd like to understand the statement below take a google for
Tess 0:f492ec86159e 140 //'ternary operators'.
Tess 0:f492ec86159e 141 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 142 {
Tess 0:f492ec86159e 143 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 144 }
Tess 0:f492ec86159e 145
Tess 0:f492ec86159e 146
Tess 0:f492ec86159e 147