Timo de Vries / Mbed 2 deprecated worknotjet

work but not jet as..

Dependencies:   MODSERIAL mbed QEI

Fork of AllMoter_test by Timo de Vries

it works for motor 1

main.cpp@7:c7bdeee1bbac, 2016-10-21 (annotated)

Committer:
Frostworks
Date:
Fri Oct 21 10:28:27 2016 +0000
Revision:
7:c7bdeee1bbac
Parent:
6:4e4df2f6157e 
motor 3 roteert niet terug en te hard

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