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@4:7b8bf798bb72, 2016-10-14 (annotated)

Committer:
Frostworks
Date:
Fri Oct 14 10:34:17 2016 +0000
Revision:
4:7b8bf798bb72
Parent:
3:dfb4b8d37bab
Child:
5:353ed56417a2
simpeler en werkender maar nog niet perfect;

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 0:2b420376e01d 3
Frostworks 1:833c73834749 4 DigitalOut M1_rotate(D2); // voltage only base rotation
Frostworks 1:833c73834749 5 PwmOut M1_Speed(D3); // voltage only base rotation
Frostworks 1:833c73834749 6
Frostworks 1:833c73834749 7 DigitalOut M2_rotate(D4); // encoder side pot 2 translation
Frostworks 1:833c73834749 8 PwmOut M2_Speed(D5); // encoder side pot 2 translation
Frostworks 1:833c73834749 9
Frostworks 1:833c73834749 10 DigitalOut M3_rotate(D7); // encoder side pot 1 spatel rotation
Frostworks 1:833c73834749 11 PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation
Frostworks 0:2b420376e01d 12
Frostworks 2:4fb733fffd1f 13 DigitalIn links(SW3);
Frostworks 2:4fb733fffd1f 14 DigitalIn rechts(SW2);
Frostworks 0:2b420376e01d 15 //DigitalOut M2_rotate(D6);
Frostworks 0:2b420376e01d 16 //PwmOut M2_Speed(D7);
Frostworks 0:2b420376e01d 17
Frostworks 1:833c73834749 18 AnalogIn pot1(A4); // pot 1 motor 1
Frostworks 1:833c73834749 19 AnalogIn pot2(A3); // pot 2 motor 3
Frostworks 1:833c73834749 20
Frostworks 0:2b420376e01d 21 MODSERIAL pc(USBTX, USBRX);
Frostworks 3:dfb4b8d37bab 22 bool draairechts;
Frostworks 3:dfb4b8d37bab 23 bool draailinks;
Frostworks 0:2b420376e01d 24
Frostworks 3:dfb4b8d37bab 25 void GetDirections()
Frostworks 3:dfb4b8d37bab 26 {
Frostworks 4:7b8bf798bb72 27 if ((rechts == 1) && (links == 1))
Frostworks 4:7b8bf798bb72 28 {}
Frostworks 4:7b8bf798bb72 29 else if ((rechts == 1) && (draailinks == 0)) {
Frostworks 3:dfb4b8d37bab 30 draairechts = !draairechts;
Frostworks 4:7b8bf798bb72 31 } else if ((rechts == 1) && (draailinks == 1)) {
Frostworks 3:dfb4b8d37bab 32 draailinks = 0;
Frostworks 4:7b8bf798bb72 33 draairechts = !draairechts;
Frostworks 4:7b8bf798bb72 34 } else if (links == 1) {
Frostworks 4:7b8bf798bb72 35 draailinks = !draailinks;
Frostworks 4:7b8bf798bb72 36 } else if ((links == 1) && (draairechts == 1)) {
Frostworks 3:dfb4b8d37bab 37 draairechts = 0;
Frostworks 3:dfb4b8d37bab 38 draailinks = !draailinks;
Frostworks 3:dfb4b8d37bab 39 }
Frostworks 4:7b8bf798bb72 40 wait(0.1);
Frostworks 3:dfb4b8d37bab 41 /*if ((rechts == 0) && (links == 0)) {
Frostworks 3:dfb4b8d37bab 42 draairechts = false;
Frostworks 3:dfb4b8d37bab 43 draailinks = false;
Frostworks 3:dfb4b8d37bab 44 } else if ((rechts == 1) && (draairechts ==false)) { //Als je rechts aanspant en hij draait nog niet naar rechts
Frostworks 3:dfb4b8d37bab 45 draairechts = true;
Frostworks 3:dfb4b8d37bab 46 draailinks = false;
Frostworks 3:dfb4b8d37bab 47 } else if ((rechts == 1) && (draairechts == true)) { //Als je rechts aanspant en hij draait al naar rechts
Frostworks 3:dfb4b8d37bab 48 draairechts = false;
Frostworks 3:dfb4b8d37bab 49 draailinks = false;
Frostworks 3:dfb4b8d37bab 50 } else if ((links == 1) && (draailinks == false)) { //Als je links aanspant en hij draait nog niet naar links
Frostworks 3:dfb4b8d37bab 51 draailinks = true;
Frostworks 3:dfb4b8d37bab 52 draairechts = false;
Frostworks 3:dfb4b8d37bab 53 } else if ((links == 1) && (draailinks == true)) { // Als je links aanspant en hij draait al naar links
Frostworks 3:dfb4b8d37bab 54 draailinks = false;
Frostworks 3:dfb4b8d37bab 55 draairechts = false;
Frostworks 3:dfb4b8d37bab 56 }
Frostworks 3:dfb4b8d37bab 57
Frostworks 3:dfb4b8d37bab 58 else if ((links == 1 && draairechts == true)) { // Als je links aanspant en hij draait nog naar rechts
Frostworks 3:dfb4b8d37bab 59 draailinks = true;
Frostworks 3:dfb4b8d37bab 60 draairechts = false;
Frostworks 3:dfb4b8d37bab 61 } else if ((rechts == 1 && draailinks == true)) { // Als je rechts aanspant en hij draait nog naar links
Frostworks 3:dfb4b8d37bab 62 draairechts = true;
Frostworks 3:dfb4b8d37bab 63 draailinks = false;
Frostworks 3:dfb4b8d37bab 64
Frostworks 3:dfb4b8d37bab 65 } else {
Frostworks 3:dfb4b8d37bab 66 draairechts = false;
Frostworks 3:dfb4b8d37bab 67 draailinks = false;
Frostworks 3:dfb4b8d37bab 68 }
Frostworks 3:dfb4b8d37bab 69 */
Frostworks 3:dfb4b8d37bab 70 }
Frostworks 0:2b420376e01d 71 int main()
Frostworks 0:2b420376e01d 72 {
Frostworks 4:7b8bf798bb72 73
Frostworks 2:4fb733fffd1f 74
Frostworks 3:dfb4b8d37bab 75 /*
Frostworks 3:dfb4b8d37bab 76 float GetReferenceVelocity() {
Frostworks 3:dfb4b8d37bab 77 // Returns reference velocity in rad/s.
Frostworks 3:dfb4b8d37bab 78 // Positive value means clockwise rotation.
Frostworks 3:dfb4b8d37bab 79 const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s
Frostworks 3:dfb4b8d37bab 80 if (a) {
Frostworks 3:dfb4b8d37bab 81 // Clockwise rotation
Frostworks 3:dfb4b8d37bab 82 referenceVelocity = potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 83
Frostworks 3:dfb4b8d37bab 84 } else {
Frostworks 3:dfb4b8d37bab 85 // Counterclockwise rotation
Frostworks 3:dfb4b8d37bab 86 referenceVelocity = -1*potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 87
Frostworks 3:dfb4b8d37bab 88 }
Frostworks 3:dfb4b8d37bab 89 return referenceVelocity;
Frostworks 2:4fb733fffd1f 90 }
Frostworks 2:4fb733fffd1f 91
Frostworks 3:dfb4b8d37bab 92 void SetMotor1(float motorValue) {
Frostworks 3:dfb4b8d37bab 93 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 94 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 95 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 96 // within range
Frostworks 3:dfb4b8d37bab 97 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 98 motor1DirectionPin=1;
Frostworks 3:dfb4b8d37bab 99 } else {
Frostworks 3:dfb4b8d37bab 100 motor1DirectionPin=0;
Frostworks 3:dfb4b8d37bab 101 }
Frostworks 3:dfb4b8d37bab 102 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 103 motor1MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 104 } else {
Frostworks 3:dfb4b8d37bab 105 motor1MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 106 }
Frostworks 2:4fb733fffd1f 107 }
Frostworks 2:4fb733fffd1f 108
Frostworks 3:dfb4b8d37bab 109 void SetMotor2(float motorValue) {
Frostworks 3:dfb4b8d37bab 110 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 111 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 112 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 113 // within range
Frostworks 3:dfb4b8d37bab 114 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 115 motor2DirectionPin=1;
Frostworks 3:dfb4b8d37bab 116 } else {
Frostworks 3:dfb4b8d37bab 117 motor2DirectionPin=0;
Frostworks 3:dfb4b8d37bab 118 }
Frostworks 3:dfb4b8d37bab 119 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 120 motor2MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 121 } else {
Frostworks 3:dfb4b8d37bab 122 motor2MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 123 }
Frostworks 2:4fb733fffd1f 124 }
Frostworks 3:dfb4b8d37bab 125
Frostworks 3:dfb4b8d37bab 126 void SetMotor3(float motorValue) {
Frostworks 3:dfb4b8d37bab 127 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 128 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 129 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 130 // within range
Frostworks 3:dfb4b8d37bab 131 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 132 motor3DirectionPin=1;
Frostworks 3:dfb4b8d37bab 133 } else {
Frostworks 3:dfb4b8d37bab 134 motor3DirectionPin=0;
Frostworks 3:dfb4b8d37bab 135 }
Frostworks 3:dfb4b8d37bab 136 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 137 motor3MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 138 } else {
Frostworks 3:dfb4b8d37bab 139 motor3MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 140 }
Frostworks 3:dfb4b8d37bab 141 }
Frostworks 3:dfb4b8d37bab 142 */
Frostworks 1:833c73834749 143
Frostworks 0:2b420376e01d 144 while (true) {
Frostworks 2:4fb733fffd1f 145 GetDirections();
Frostworks 4:7b8bf798bb72 146 if (draairechts == true) {
Frostworks 4:7b8bf798bb72 147 M1_Speed = 0.5;
Frostworks 4:7b8bf798bb72 148 M1_rotate = 0;
Frostworks 4:7b8bf798bb72 149 } else if (draailinks == true) {
Frostworks 4:7b8bf798bb72 150 M1_Speed = 0.5;
Frostworks 4:7b8bf798bb72 151 M1_rotate = 1;
Frostworks 4:7b8bf798bb72 152 } else if ((draailinks == false) && (draairechts == false)) {
Frostworks 4:7b8bf798bb72 153 M1_Speed = 0;
Frostworks 4:7b8bf798bb72 154 }
Frostworks 4:7b8bf798bb72 155
Frostworks 4:7b8bf798bb72 156
Frostworks 4:7b8bf798bb72 157
Frostworks 4:7b8bf798bb72 158 /*
Frostworks 3:dfb4b8d37bab 159 if ((draailinks == true) && (draairechts == false)) {
Frostworks 2:4fb733fffd1f 160 M1_Speed = 0.5;
Frostworks 3:dfb4b8d37bab 161 M1_rotate = 0;
Frostworks 3:dfb4b8d37bab 162 } else if ((draairechts == true) && (draailinks == false)) {
Frostworks 2:4fb733fffd1f 163 M1_Speed = 0.5;
Frostworks 3:dfb4b8d37bab 164 M1_rotate = 1;
Frostworks 2:4fb733fffd1f 165 } else {
Frostworks 2:4fb733fffd1f 166 M1_Speed = 0;
Frostworks 1:833c73834749 167 }
Frostworks 4:7b8bf798bb72 168 */
Frostworks 3:dfb4b8d37bab 169
Frostworks 2:4fb733fffd1f 170 /*
Frostworks 1:833c73834749 171 float M2 = M2_rotate.read();
Frostworks 1:833c73834749 172 float potje1 = pot1.read();
Frostworks 1:833c73834749 173 float potje2 = pot2.read();
Frostworks 1:833c73834749 174 M1_Speed.write(potje1);
Frostworks 1:833c73834749 175 M3_Speed.write(potje2);
Frostworks 0:2b420376e01d 176 wait(0.1);
Frostworks 1:833c73834749 177 pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
Frostworks 2:4fb733fffd1f 178 */
Frostworks 3:dfb4b8d37bab 179
Frostworks 0:2b420376e01d 180 }
Frostworks 4:7b8bf798bb72 181
Frostworks 1:833c73834749 182 }