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@7:1f88215b504c, 2013-10-30 (annotated)

Committer:
Tess
Date:
Wed Oct 30 12:25:09 2013 +0000
Revision:
7:1f88215b504c
Parent:
6:2cfdda6721ab
Child:
8:62e968f78878
new with only the calibration stuff

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 2:83dd9068b6c5 52 float setpoint_beginA;
Tess 2:83dd9068b6c5 53 float setpoint_beginB;
Tess 2:83dd9068b6c5 54 float setpoint_rechtsonderA;
Tess 2:83dd9068b6c5 55 float setpoint_rechtsonderB;
Tess 0:f492ec86159e 56 /* variable to store pwm value in*/
Tess 0:f492ec86159e 57 float pwm_to_motorA;
vsluiter 4:8344a3edd96c 58 float pwm_to_begin_motorA = 0;
vsluiter 4:8344a3edd96c 59 float pwm_to_begin_motorB = 0;
Tess 0:f492ec86159e 60 float pwm_to_motorB;
Tess 2:83dd9068b6c5 61 float pwm_to_rechtsonder_motorA;
Tess 2:83dd9068b6c5 62 float pwm_to_rechtsonder_motorB;
Tess 1:7cafc9042056 63 int32_t positionmotorA_t0;
Tess 1:7cafc9042056 64 int32_t positionmotorB_t0;
Tess 1:7cafc9042056 65 int32_t positionmotorA_t_1;
Tess 1:7cafc9042056 66 int32_t positionmotorB_t_1;
Tess 2:83dd9068b6c5 67 int32_t positiondifference_motorA;
Tess 2:83dd9068b6c5 68 int32_t positiondifference_motorB;
Tess 1:7cafc9042056 69
Tess 0:f492ec86159e 70 //START OF CODE
Tess 0:f492ec86159e 71
Tess 5:512dd4044486 72 /*Set the baudrate (use this number in RealTerm too!) */
Tess 0:f492ec86159e 73 pc.baud(921600);
Tess 0:f492ec86159e 74
Tess 7:1f88215b504c 75 // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
Tess 7:1f88215b504c 76 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
Tess 7:1f88215b504c 77
Tess 7:1f88215b504c 78 motordirB.write(0);
Tess 2:83dd9068b6c5 79 pwm_motorB.write(.08);
Tess 1:7cafc9042056 80 positionmotorB_t0 = motorB.getPosition();
Tess 1:7cafc9042056 81 do {
Tess 2:83dd9068b6c5 82 wait(0.2);
Tess 7:1f88215b504c 83 positionmotorB_t_1 = positionmotorB_t0 ;
Tess 1:7cafc9042056 84 positionmotorB_t0 = motorB.getPosition();
Tess 2:83dd9068b6c5 85 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
Tess 2:83dd9068b6c5 86 } while(positiondifference_motorB > 10);
Tess 2:83dd9068b6c5 87 motorB.setPosition(0);
Tess 2:83dd9068b6c5 88 pwm_motorB.write(0);
Tess 1:7cafc9042056 89
Tess 7:1f88215b504c 90 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 91
Tess 2:83dd9068b6c5 92 motordirA.write(1);
Tess 2:83dd9068b6c5 93 pwm_motorA.write(.08);
Tess 2:83dd9068b6c5 94 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 95 do {
Tess 2:83dd9068b6c5 96 wait(0.2);
Tess 2:83dd9068b6c5 97 positionmotorA_t_1 = positionmotorA_t0 ;
Tess 2:83dd9068b6c5 98 positionmotorA_t0 = motorA.getPosition();
Tess 2:83dd9068b6c5 99 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
Tess 2:83dd9068b6c5 100 } while(positiondifference_motorA > 10);
Tess 2:83dd9068b6c5 101 motorA.setPosition(0);
Tess 2:83dd9068b6c5 102 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 103
Tess 7:1f88215b504c 104 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 105
Tess 7:1f88215b504c 106 // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
Tess 5:512dd4044486 107 // Hierna moet motor B 21.6 (192) graden naar links. Dus eerst motor A en dan motor B.
Tess 0:f492ec86159e 108
Tess 7:1f88215b504c 109 motordirA.write(0);
Tess 7:1f88215b504c 110 pwm_motorA.write(.08);
Tess 5:512dd4044486 111 do {
Tess 7:1f88215b504c 112 setpoint_beginA = -63;
Tess 7:1f88215b504c 113 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
Tess 7:1f88215b504c 114 pc.printf("s: %f", pwm_to_begin_motorA);
Tess 7:1f88215b504c 115 } while(pwm_to_begin_motorA <= 0);
Tess 7:1f88215b504c 116 {
Tess 7:1f88215b504c 117 wait(0.5);
Tess 2:83dd9068b6c5 118 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 119 motordirA.write(0);
Tess 5:512dd4044486 120 pwm_motorA.write(pwm_to_begin_motorA);
Tess 2:83dd9068b6c5 121 }
Tess 3:c4df318913b8 122 motorA.setPosition(0);
Tess 3:c4df318913b8 123 pwm_motorA.write(0);
Tess 2:83dd9068b6c5 124
Tess 7:1f88215b504c 125 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 7:1f88215b504c 126
Tess 7:1f88215b504c 127 motordirB.write(1);
Tess 7:1f88215b504c 128 pwm_motorB.write(.08);
Tess 7:1f88215b504c 129 do {
Tess 7:1f88215b504c 130 setpoint_beginB = 192;
Tess 7:1f88215b504c 131 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 7:1f88215b504c 132 pc.printf("s: %f", pwm_to_begin_motorB);
Tess 7:1f88215b504c 133 } while(pwm_to_begin_motorB <= 0);
Tess 7:1f88215b504c 134 {
Tess 7:1f88215b504c 135 wait(0.5);
Tess 7:1f88215b504c 136 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 3:c4df318913b8 137 motordirB.write(1);
Tess 5:512dd4044486 138 pwm_motorB.write(pwm_to_begin_motorB);
Tess 3:c4df318913b8 139 }
Tess 3:c4df318913b8 140 motorB.setPosition(0);
Tess 3:c4df318913b8 141 pwm_motorB.write(0);
Tess 2:83dd9068b6c5 142
Tess 7:1f88215b504c 143 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
Tess 3:c4df318913b8 144
Tess 7:1f88215b504c 145 // Hierna willen we de motor van zijn x-as naar de rechtsonder positie van A4 te krijgen. Volgorde van motoren maakt nu niet uit.
Tess 7:1f88215b504c 146 // nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460
Tess 3:c4df318913b8 147
Tess 7:1f88215b504c 148 motordirB.write(1);
Tess 7:1f88215b504c 149 motordirA.write(0);
Tess 7:1f88215b504c 150 pwm_motorB.write(0.08);
Tess 7:1f88215b504c 151 pwm_motorA.write(0.08);
Tess 7:1f88215b504c 152 do {
Tess 7:1f88215b504c 153 setpoint_beginA = -1000; //-532
Tess 7:1f88215b504c 154 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
Tess 7:1f88215b504c 155 setpoint_beginB = 1000; //460
Tess 7:1f88215b504c 156 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
Tess 7:1f88215b504c 157 } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); {
Tess 7:1f88215b504c 158 wait(2);
Tess 7:1f88215b504c 159 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
Tess 7:1f88215b504c 160 motordirB.write(1);
Tess 7:1f88215b504c 161 pwm_motorB.write(pwm_to_begin_motorB);
Tess 3:c4df318913b8 162 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
Tess 2:83dd9068b6c5 163 motordirA.write(0);
Tess 7:1f88215b504c 164 pwm_motorA.write(pwm_to_begin_motorA);
Tess 3:c4df318913b8 165 }
Tess 3:c4df318913b8 166 pwm_motorA.write(0);
Tess 3:c4df318913b8 167 pwm_motorB.write(0);
Tess 0:f492ec86159e 168
Tess 0:f492ec86159e 169
Tess 0:f492ec86159e 170 }
Tess 0:f492ec86159e 171
Tess 0:f492ec86159e 172
Tess 0:f492ec86159e 173 void keep_in_range(float * in, float min, float max)
Tess 0:f492ec86159e 174 {
Tess 0:f492ec86159e 175 *in > min ? *in < max? : *in = max: *in = min;
Tess 0:f492ec86159e 176 }
Tess 0:f492ec86159e 177
Tess 0:f492ec86159e 178
Tess 0:f492ec86159e 179
Tess 7:1f88215b504c 180
Tess 7:1f88215b504c 181
Tess 7:1f88215b504c 182
Tess 7:1f88215b504c 183