Timo de Vries / Mbed 2 deprecated AllMotor_test

Alle functies van de motoren werken

Dependencies:   MODSERIAL QEI mbed

Fork of worknotjet by Timo de Vries

main.cpp@8:8a3ab396853f, 2016-10-21 (annotated)

Committer:
Frostworks
Date:
Fri Oct 21 12:40:43 2016 +0000
Revision:
8:8a3ab396853f
Parent:
7:c7bdeee1bbac 
De motoren voldoen in functie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Frostworks 0:2b420376e01d 1 #include "mbed.h"
Frostworks 0:2b420376e01d 2 #include "MODSERIAL.h"
Frostworks 6:4e4df2f6157e 3 #include "QEI.h"
Frostworks 0:2b420376e01d 4
Frostworks 8:8a3ab396853f 5 DigitalOut led_g(LED_GREEN);
Frostworks 8:8a3ab396853f 6 DigitalOut led_b(LED_BLUE);
Frostworks 8:8a3ab396853f 7 DigitalOut led_r(LED_RED);
Frostworks 6:4e4df2f6157e 8
Frostworks 6:4e4df2f6157e 9 DigitalOut M1_Rotate(D2); // voltage only base rotation
Frostworks 1:833c73834749 10 PwmOut M1_Speed(D3); // voltage only base rotation
Frostworks 1:833c73834749 11
Frostworks 6:4e4df2f6157e 12 MODSERIAL pc(USBTX, USBRX);
Frostworks 6:4e4df2f6157e 13
Frostworks 6:4e4df2f6157e 14 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
Frostworks 6:4e4df2f6157e 15 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
Frostworks 6:4e4df2f6157e 16 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
Frostworks 6:4e4df2f6157e 17
Frostworks 6:4e4df2f6157e 18 DigitalOut M2_Rotate(D4); // encoder side pot 2 translation
Frostworks 1:833c73834749 19 PwmOut M2_Speed(D5); // encoder side pot 2 translation
Frostworks 1:833c73834749 20
Frostworks 6:4e4df2f6157e 21 DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation
Frostworks 1:833c73834749 22 PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation
Frostworks 0:2b420376e01d 23
Frostworks 2:4fb733fffd1f 24 DigitalIn links(SW3);
Frostworks 2:4fb733fffd1f 25 DigitalIn rechts(SW2);
Frostworks 0:2b420376e01d 26 //DigitalOut M2_rotate(D6);
Frostworks 0:2b420376e01d 27 //PwmOut M2_Speed(D7);
Frostworks 0:2b420376e01d 28
Frostworks 1:833c73834749 29 AnalogIn pot1(A4); // pot 1 motor 1
Frostworks 1:833c73834749 30 AnalogIn pot2(A3); // pot 2 motor 3
Frostworks 1:833c73834749 31
Frostworks 3:dfb4b8d37bab 32 bool draairechts;
Frostworks 3:dfb4b8d37bab 33 bool draailinks;
Frostworks 6:4e4df2f6157e 34 bool turn = 0;
Frostworks 5:353ed56417a2 35 float waiter = 0.1;
Frostworks 8:8a3ab396853f 36 float afstand = 200;
Frostworks 6:4e4df2f6157e 37 float translation = 0;
Frostworks 7:c7bdeee1bbac 38 float degrees3 = 0;
Frostworks 7:c7bdeee1bbac 39
Frostworks 7:c7bdeee1bbac 40 float Puls_degree = (8400/360);
Frostworks 7:c7bdeee1bbac 41 float wheel1 = 16;
Frostworks 7:c7bdeee1bbac 42 float wheel2 = 31;
Frostworks 7:c7bdeee1bbac 43 float wheel3 = 41;
Frostworks 7:c7bdeee1bbac 44 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
Frostworks 7:c7bdeee1bbac 45 float pi = 3.14159265359;
Frostworks 7:c7bdeee1bbac 46
Frostworks 3:dfb4b8d37bab 47 void GetDirections()
Frostworks 3:dfb4b8d37bab 48 {
Frostworks 5:353ed56417a2 49 pc.baud(115200);
Frostworks 6:4e4df2f6157e 50 if ((rechts == 0) && (links == 0) && (turn == 0)) {
Frostworks 5:353ed56417a2 51 draailinks = 0;
Frostworks 5:353ed56417a2 52 draairechts = 0;
Frostworks 6:4e4df2f6157e 53 turn = 1;
Frostworks 5:353ed56417a2 54 pc.printf("begin de actie \n \r ");
Frostworks 5:353ed56417a2 55 wait(waiter);
Frostworks 5:353ed56417a2 56
Frostworks 6:4e4df2f6157e 57 } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
Frostworks 5:353ed56417a2 58 draailinks = 0;
Frostworks 5:353ed56417a2 59 draairechts = 0;
Frostworks 6:4e4df2f6157e 60 turn = 0;
Frostworks 5:353ed56417a2 61 pc.printf("breek de actie af \n \r ");
Frostworks 5:353ed56417a2 62 wait(waiter);
Frostworks 6:4e4df2f6157e 63 } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
Frostworks 5:353ed56417a2 64
Frostworks 6:4e4df2f6157e 65 } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
Frostworks 5:353ed56417a2 66 /* if the right button is pressed and the motor isn't rotating to the left,
Frostworks 5:353ed56417a2 67 then start rotating to the right etc*/
Frostworks 3:dfb4b8d37bab 68 draairechts = !draairechts;
Frostworks 5:353ed56417a2 69 pc.printf("draai naar rechts \n \r ");
Frostworks 5:353ed56417a2 70 wait(waiter);
Frostworks 6:4e4df2f6157e 71 } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
Frostworks 3:dfb4b8d37bab 72 draailinks = 0;
Frostworks 4:7b8bf798bb72 73 draairechts = !draairechts;
Frostworks 5:353ed56417a2 74 pc.printf("draai naar rechts na links \n \r ");
Frostworks 5:353ed56417a2 75 wait(waiter);
Frostworks 6:4e4df2f6157e 76 } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
Frostworks 4:7b8bf798bb72 77 draailinks = !draailinks;
Frostworks 5:353ed56417a2 78 pc.printf("draai naar links \n \r ");
Frostworks 5:353ed56417a2 79 wait(waiter);
Frostworks 6:4e4df2f6157e 80 } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
Frostworks 3:dfb4b8d37bab 81 draairechts = 0;
Frostworks 3:dfb4b8d37bab 82 draailinks = !draailinks;
Frostworks 5:353ed56417a2 83 pc.printf("draai naar links na rechts \n \r ");
Frostworks 5:353ed56417a2 84 wait(waiter);
Frostworks 3:dfb4b8d37bab 85 }
Frostworks 5:353ed56417a2 86 wait(2*waiter);
Frostworks 3:dfb4b8d37bab 87 }
Frostworks 6:4e4df2f6157e 88
Frostworks 7:c7bdeee1bbac 89 float GetPositionM2()
Frostworks 7:c7bdeee1bbac 90 {
Frostworks 7:c7bdeee1bbac 91 float pulses2 = motor2.getPulses();
Frostworks 7:c7bdeee1bbac 92 float degrees2 = (pulses2/Puls_degree);
Frostworks 7:c7bdeee1bbac 93 float radians2 = (degrees2/360)*2*pi;
Frostworks 7:c7bdeee1bbac 94 float translation = ((radians2/overbrenging)*32.25);
Frostworks 7:c7bdeee1bbac 95
Frostworks 7:c7bdeee1bbac 96 return translation;
Frostworks 7:c7bdeee1bbac 97 }
Frostworks 7:c7bdeee1bbac 98 float GetRotationM3()
Frostworks 6:4e4df2f6157e 99 {
Frostworks 7:c7bdeee1bbac 100 float pulses3 = motor3.getPulses();
Frostworks 7:c7bdeee1bbac 101 float degrees3 = (pulses3/Puls_degree);
Frostworks 7:c7bdeee1bbac 102 float radians3 = (degrees3/360)*2*pi;
Frostworks 7:c7bdeee1bbac 103
Frostworks 7:c7bdeee1bbac 104 return degrees3;
Frostworks 7:c7bdeee1bbac 105 }
Frostworks 7:c7bdeee1bbac 106 void GoBack()
Frostworks 7:c7bdeee1bbac 107 {
Frostworks 7:c7bdeee1bbac 108 while (GetPositionM2() > 0) {
Frostworks 8:8a3ab396853f 109 M3_Speed = 0;
Frostworks 8:8a3ab396853f 110 M2_Speed = 1;
Frostworks 7:c7bdeee1bbac 111 M2_Rotate = 0;
Frostworks 7:c7bdeee1bbac 112 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 8:8a3ab396853f 113 led_r = 0;
Frostworks 7:c7bdeee1bbac 114 }
Frostworks 7:c7bdeee1bbac 115 M2_Speed = 0;
Frostworks 6:4e4df2f6157e 116
Frostworks 6:4e4df2f6157e 117
Frostworks 8:8a3ab396853f 118 while (GetRotationM3() > 0) {
Frostworks 7:c7bdeee1bbac 119 M3_Rotate = 1;
Frostworks 7:c7bdeee1bbac 120 M3_Speed = 0.2;
Frostworks 8:8a3ab396853f 121 led_r = 1;
Frostworks 8:8a3ab396853f 122 led_b = 0;
Frostworks 8:8a3ab396853f 123 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 8:8a3ab396853f 124
Frostworks 7:c7bdeee1bbac 125 }
Frostworks 8:8a3ab396853f 126 M3_Speed = 0;
Frostworks 7:c7bdeee1bbac 127
Frostworks 8:8a3ab396853f 128 turn = 0;
Frostworks 7:c7bdeee1bbac 129
Frostworks 8:8a3ab396853f 130
Frostworks 6:4e4df2f6157e 131 }
Frostworks 6:4e4df2f6157e 132 void Burgerflip()
Frostworks 6:4e4df2f6157e 133 {
Frostworks 7:c7bdeee1bbac 134 if (GetPositionM2() > afstand) {
Frostworks 7:c7bdeee1bbac 135 M3_Speed = 0.2;
Frostworks 7:c7bdeee1bbac 136 M3_Rotate = 0;
Frostworks 7:c7bdeee1bbac 137 M2_Speed = 0;
Frostworks 7:c7bdeee1bbac 138 } else if (GetPositionM2() < afstand) {
Frostworks 8:8a3ab396853f 139 M2_Speed = 1;
Frostworks 7:c7bdeee1bbac 140 M2_Rotate = 1;
Frostworks 8:8a3ab396853f 141
Frostworks 7:c7bdeee1bbac 142 }
Frostworks 8:8a3ab396853f 143 if (GetRotationM3() > 150) {
Frostworks 7:c7bdeee1bbac 144 GoBack();
Frostworks 7:c7bdeee1bbac 145 }
Frostworks 7:c7bdeee1bbac 146 }
Frostworks 6:4e4df2f6157e 147
Frostworks 0:2b420376e01d 148 int main()
Frostworks 0:2b420376e01d 149 {
Frostworks 8:8a3ab396853f 150 led_g = 1;
Frostworks 8:8a3ab396853f 151 led_b = 1;
Frostworks 8:8a3ab396853f 152 led_r = 1;
Frostworks 8:8a3ab396853f 153
Frostworks 6:4e4df2f6157e 154 while (true) {
Frostworks 8:8a3ab396853f 155
Frostworks 6:4e4df2f6157e 156 GetDirections();
Frostworks 6:4e4df2f6157e 157 if (draairechts == true) {
Frostworks 7:c7bdeee1bbac 158 M1_Speed = 0.2;
Frostworks 6:4e4df2f6157e 159 M1_Rotate = 0;
Frostworks 6:4e4df2f6157e 160 } else if (draailinks == true) {
Frostworks 7:c7bdeee1bbac 161 M1_Speed = 0.2;
Frostworks 6:4e4df2f6157e 162 M1_Rotate = 1;
Frostworks 6:4e4df2f6157e 163 } else if (turn == 1) {
Frostworks 6:4e4df2f6157e 164 /*M2_Speed = 0.5;
Frostworks 6:4e4df2f6157e 165 M2_Rotate = 1;
Frostworks 6:4e4df2f6157e 166 M3_Speed = 0.5;
Frostworks 6:4e4df2f6157e 167 M3_Rotate = 1;*/
Frostworks 6:4e4df2f6157e 168 Burgerflip();
Frostworks 6:4e4df2f6157e 169 } else if (turn == 0) {
Frostworks 6:4e4df2f6157e 170 M2_Speed = 0;
Frostworks 6:4e4df2f6157e 171 M3_Speed = 0;
Frostworks 6:4e4df2f6157e 172 }
Frostworks 6:4e4df2f6157e 173 if ((draailinks == false) && (draairechts == false)) {
Frostworks 6:4e4df2f6157e 174 M1_Speed = 0;
Frostworks 6:4e4df2f6157e 175 }
Frostworks 7:c7bdeee1bbac 176 pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
Frostworks 6:4e4df2f6157e 177 /* pulses = 8400 */
Frostworks 6:4e4df2f6157e 178 }
Frostworks 6:4e4df2f6157e 179 }
Frostworks 6:4e4df2f6157e 180 /*
Frostworks 3:dfb4b8d37bab 181 float GetReferenceVelocity() {
Frostworks 3:dfb4b8d37bab 182 // Returns reference velocity in rad/s.
Frostworks 3:dfb4b8d37bab 183 // Positive value means clockwise rotation.
Frostworks 3:dfb4b8d37bab 184 const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s
Frostworks 3:dfb4b8d37bab 185 if (a) {
Frostworks 3:dfb4b8d37bab 186 // Clockwise rotation
Frostworks 3:dfb4b8d37bab 187 referenceVelocity = potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 188
Frostworks 3:dfb4b8d37bab 189 } else {
Frostworks 3:dfb4b8d37bab 190 // Counterclockwise rotation
Frostworks 3:dfb4b8d37bab 191 referenceVelocity = -1*potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 192
Frostworks 3:dfb4b8d37bab 193 }
Frostworks 3:dfb4b8d37bab 194 return referenceVelocity;
Frostworks 2:4fb733fffd1f 195 }
Frostworks 2:4fb733fffd1f 196
Frostworks 3:dfb4b8d37bab 197 void SetMotor1(float motorValue) {
Frostworks 3:dfb4b8d37bab 198 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 199 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 200 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 201 // within range
Frostworks 3:dfb4b8d37bab 202 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 203 motor1DirectionPin=1;
Frostworks 3:dfb4b8d37bab 204 } else {
Frostworks 3:dfb4b8d37bab 205 motor1DirectionPin=0;
Frostworks 3:dfb4b8d37bab 206 }
Frostworks 3:dfb4b8d37bab 207 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 208 motor1MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 209 } else {
Frostworks 3:dfb4b8d37bab 210 motor1MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 211 }
Frostworks 2:4fb733fffd1f 212 }
Frostworks 2:4fb733fffd1f 213
Frostworks 3:dfb4b8d37bab 214 void SetMotor2(float motorValue) {
Frostworks 3:dfb4b8d37bab 215 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 216 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 217 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 218 // within range
Frostworks 3:dfb4b8d37bab 219 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 220 motor2DirectionPin=1;
Frostworks 3:dfb4b8d37bab 221 } else {
Frostworks 3:dfb4b8d37bab 222 motor2DirectionPin=0;
Frostworks 3:dfb4b8d37bab 223 }
Frostworks 3:dfb4b8d37bab 224 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 225 motor2MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 226 } else {
Frostworks 3:dfb4b8d37bab 227 motor2MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 228 }
Frostworks 2:4fb733fffd1f 229 }
Frostworks 3:dfb4b8d37bab 230
Frostworks 3:dfb4b8d37bab 231 void SetMotor3(float motorValue) {
Frostworks 3:dfb4b8d37bab 232 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 233 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 234 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 235 // within range
Frostworks 3:dfb4b8d37bab 236 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 237 motor3DirectionPin=1;
Frostworks 3:dfb4b8d37bab 238 } else {
Frostworks 3:dfb4b8d37bab 239 motor3DirectionPin=0;
Frostworks 3:dfb4b8d37bab 240 }
Frostworks 3:dfb4b8d37bab 241 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 242 motor3MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 243 } else {
Frostworks 3:dfb4b8d37bab 244 motor3MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 245 }
Frostworks 3:dfb4b8d37bab 246 }
Frostworks 6:4e4df2f6157e 247 */