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@6:4e4df2f6157e, 2016-10-21 (annotated)

Committer:
Frostworks
Date:
Fri Oct 21 08:56:33 2016 +0000
Revision:
6:4e4df2f6157e
Parent:
5:353ed56417a2
Child:
7:c7bdeee1bbac
beginnen met burger omdraai gedeelte;

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 6:4e4df2f6157e 33 float afstand = 150;
Frostworks 6:4e4df2f6157e 34 float translation = 0;
Frostworks 3:dfb4b8d37bab 35 void GetDirections()
Frostworks 3:dfb4b8d37bab 36 {
Frostworks 5:353ed56417a2 37 pc.baud(115200);
Frostworks 6:4e4df2f6157e 38 if ((rechts == 0) && (links == 0) && (turn == 0)) {
Frostworks 5:353ed56417a2 39 draailinks = 0;
Frostworks 5:353ed56417a2 40 draairechts = 0;
Frostworks 6:4e4df2f6157e 41 turn = 1;
Frostworks 5:353ed56417a2 42 pc.printf("begin de actie \n \r ");
Frostworks 5:353ed56417a2 43 wait(waiter);
Frostworks 5:353ed56417a2 44
Frostworks 6:4e4df2f6157e 45 } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
Frostworks 5:353ed56417a2 46 draailinks = 0;
Frostworks 5:353ed56417a2 47 draairechts = 0;
Frostworks 6:4e4df2f6157e 48 turn = 0;
Frostworks 5:353ed56417a2 49 pc.printf("breek de actie af \n \r ");
Frostworks 5:353ed56417a2 50 wait(waiter);
Frostworks 6:4e4df2f6157e 51 } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
Frostworks 5:353ed56417a2 52
Frostworks 6:4e4df2f6157e 53 } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
Frostworks 5:353ed56417a2 54 /* if the right button is pressed and the motor isn't rotating to the left,
Frostworks 5:353ed56417a2 55 then start rotating to the right etc*/
Frostworks 3:dfb4b8d37bab 56 draairechts = !draairechts;
Frostworks 5:353ed56417a2 57 pc.printf("draai naar rechts \n \r ");
Frostworks 5:353ed56417a2 58 wait(waiter);
Frostworks 6:4e4df2f6157e 59 } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
Frostworks 3:dfb4b8d37bab 60 draailinks = 0;
Frostworks 4:7b8bf798bb72 61 draairechts = !draairechts;
Frostworks 5:353ed56417a2 62 pc.printf("draai naar rechts na links \n \r ");
Frostworks 5:353ed56417a2 63 wait(waiter);
Frostworks 6:4e4df2f6157e 64 } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
Frostworks 4:7b8bf798bb72 65 draailinks = !draailinks;
Frostworks 5:353ed56417a2 66 pc.printf("draai naar links \n \r ");
Frostworks 5:353ed56417a2 67 wait(waiter);
Frostworks 6:4e4df2f6157e 68 } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
Frostworks 3:dfb4b8d37bab 69 draairechts = 0;
Frostworks 3:dfb4b8d37bab 70 draailinks = !draailinks;
Frostworks 5:353ed56417a2 71 pc.printf("draai naar links na rechts \n \r ");
Frostworks 5:353ed56417a2 72 wait(waiter);
Frostworks 3:dfb4b8d37bab 73 }
Frostworks 5:353ed56417a2 74 wait(2*waiter);
Frostworks 3:dfb4b8d37bab 75 }
Frostworks 6:4e4df2f6157e 76
Frostworks 6:4e4df2f6157e 77 void GetPosition()
Frostworks 6:4e4df2f6157e 78 {
Frostworks 6:4e4df2f6157e 79 float pulses = motor2.getPulses();
Frostworks 6:4e4df2f6157e 80 float Puls_degree = (8400/360);
Frostworks 6:4e4df2f6157e 81 float degrees = (pulses/Puls_degree);
Frostworks 6:4e4df2f6157e 82 float wheel1 = 16;
Frostworks 6:4e4df2f6157e 83 float wheel2 = 31;
Frostworks 6:4e4df2f6157e 84 float wheel3 = 41;
Frostworks 6:4e4df2f6157e 85 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
Frostworks 6:4e4df2f6157e 86 float pi = 3.14159265359;
Frostworks 6:4e4df2f6157e 87 float radians = (degrees/360)*2*pi;
Frostworks 6:4e4df2f6157e 88 float translation = ((radians/overbrenging)*32.25);
Frostworks 6:4e4df2f6157e 89 pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses);
Frostworks 6:4e4df2f6157e 90
Frostworks 6:4e4df2f6157e 91
Frostworks 6:4e4df2f6157e 92 }
Frostworks 6:4e4df2f6157e 93 void Burgerflip()
Frostworks 6:4e4df2f6157e 94 {
Frostworks 6:4e4df2f6157e 95 M2_Speed = 0.5;
Frostworks 6:4e4df2f6157e 96 M2_Rotate = 1;
Frostworks 6:4e4df2f6157e 97 M3_Speed = 0.5;
Frostworks 6:4e4df2f6157e 98 M3_Rotate = 1;
Frostworks 6:4e4df2f6157e 99
Frostworks 6:4e4df2f6157e 100 }
Frostworks 0:2b420376e01d 101 int main()
Frostworks 0:2b420376e01d 102 {
Frostworks 6:4e4df2f6157e 103 while (true) {
Frostworks 6:4e4df2f6157e 104 GetDirections();
Frostworks 6:4e4df2f6157e 105 if (draairechts == true) {
Frostworks 6:4e4df2f6157e 106 M1_Speed = 0.5;
Frostworks 6:4e4df2f6157e 107 M1_Rotate = 0;
Frostworks 6:4e4df2f6157e 108 } else if (draailinks == true) {
Frostworks 6:4e4df2f6157e 109 M1_Speed = 0.5;
Frostworks 6:4e4df2f6157e 110 M1_Rotate = 1;
Frostworks 6:4e4df2f6157e 111 } else if (turn == 1) {
Frostworks 6:4e4df2f6157e 112 /*M2_Speed = 0.5;
Frostworks 6:4e4df2f6157e 113 M2_Rotate = 1;
Frostworks 6:4e4df2f6157e 114 M3_Speed = 0.5;
Frostworks 6:4e4df2f6157e 115 M3_Rotate = 1;*/
Frostworks 6:4e4df2f6157e 116 Burgerflip();
Frostworks 6:4e4df2f6157e 117 } else if (turn == 0) {
Frostworks 6:4e4df2f6157e 118 M2_Speed = 0;
Frostworks 6:4e4df2f6157e 119 M3_Speed = 0;
Frostworks 6:4e4df2f6157e 120 }
Frostworks 6:4e4df2f6157e 121 if ((draailinks == false) && (draairechts == false)) {
Frostworks 6:4e4df2f6157e 122 M1_Speed = 0;
Frostworks 6:4e4df2f6157e 123 }
Frostworks 6:4e4df2f6157e 124 GetPosition();
Frostworks 6:4e4df2f6157e 125 /* pulses = 8400 */
Frostworks 6:4e4df2f6157e 126 }
Frostworks 6:4e4df2f6157e 127 }
Frostworks 6:4e4df2f6157e 128 /*
Frostworks 3:dfb4b8d37bab 129 float GetReferenceVelocity() {
Frostworks 3:dfb4b8d37bab 130 // Returns reference velocity in rad/s.
Frostworks 3:dfb4b8d37bab 131 // Positive value means clockwise rotation.
Frostworks 3:dfb4b8d37bab 132 const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s
Frostworks 3:dfb4b8d37bab 133 if (a) {
Frostworks 3:dfb4b8d37bab 134 // Clockwise rotation
Frostworks 3:dfb4b8d37bab 135 referenceVelocity = potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 136
Frostworks 3:dfb4b8d37bab 137 } else {
Frostworks 3:dfb4b8d37bab 138 // Counterclockwise rotation
Frostworks 3:dfb4b8d37bab 139 referenceVelocity = -1*potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 140
Frostworks 3:dfb4b8d37bab 141 }
Frostworks 3:dfb4b8d37bab 142 return referenceVelocity;
Frostworks 2:4fb733fffd1f 143 }
Frostworks 2:4fb733fffd1f 144
Frostworks 3:dfb4b8d37bab 145 void SetMotor1(float motorValue) {
Frostworks 3:dfb4b8d37bab 146 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 147 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 148 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 149 // within range
Frostworks 3:dfb4b8d37bab 150 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 151 motor1DirectionPin=1;
Frostworks 3:dfb4b8d37bab 152 } else {
Frostworks 3:dfb4b8d37bab 153 motor1DirectionPin=0;
Frostworks 3:dfb4b8d37bab 154 }
Frostworks 3:dfb4b8d37bab 155 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 156 motor1MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 157 } else {
Frostworks 3:dfb4b8d37bab 158 motor1MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 159 }
Frostworks 2:4fb733fffd1f 160 }
Frostworks 2:4fb733fffd1f 161
Frostworks 3:dfb4b8d37bab 162 void SetMotor2(float motorValue) {
Frostworks 3:dfb4b8d37bab 163 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 164 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 165 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 166 // within range
Frostworks 3:dfb4b8d37bab 167 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 168 motor2DirectionPin=1;
Frostworks 3:dfb4b8d37bab 169 } else {
Frostworks 3:dfb4b8d37bab 170 motor2DirectionPin=0;
Frostworks 3:dfb4b8d37bab 171 }
Frostworks 3:dfb4b8d37bab 172 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 173 motor2MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 174 } else {
Frostworks 3:dfb4b8d37bab 175 motor2MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 176 }
Frostworks 2:4fb733fffd1f 177 }
Frostworks 3:dfb4b8d37bab 178
Frostworks 3:dfb4b8d37bab 179 void SetMotor3(float motorValue) {
Frostworks 3:dfb4b8d37bab 180 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 181 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 182 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 183 // within range
Frostworks 3:dfb4b8d37bab 184 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 185 motor3DirectionPin=1;
Frostworks 3:dfb4b8d37bab 186 } else {
Frostworks 3:dfb4b8d37bab 187 motor3DirectionPin=0;
Frostworks 3:dfb4b8d37bab 188 }
Frostworks 3:dfb4b8d37bab 189 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 190 motor3MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 191 } else {
Frostworks 3:dfb4b8d37bab 192 motor3MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 193 }
Frostworks 3:dfb4b8d37bab 194 }
Frostworks 6:4e4df2f6157e 195 */