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

Committer:
Frostworks
Date:
Fri Oct 14 10:09:44 2016 +0000
Revision:
3:dfb4b8d37bab
Parent:
2:4fb733fffd1f
Child:
4:7b8bf798bb72
werkt nog niet;

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 3:dfb4b8d37bab 27 if (rechts == 1) {
Frostworks 3:dfb4b8d37bab 28 draairechts = !draairechts;
Frostworks 3:dfb4b8d37bab 29 draailinks = 0;
Frostworks 3:dfb4b8d37bab 30 } else if (rechts == 0) {
Frostworks 3:dfb4b8d37bab 31 draairechts = 0;
Frostworks 3:dfb4b8d37bab 32 }
Frostworks 3:dfb4b8d37bab 33
Frostworks 3:dfb4b8d37bab 34 if (links == 1) {
Frostworks 3:dfb4b8d37bab 35 draailinks = !draailinks;
Frostworks 3:dfb4b8d37bab 36 draairechts = 0;
Frostworks 3:dfb4b8d37bab 37 } else if (links == 0) {
Frostworks 3:dfb4b8d37bab 38 draailinks = 0;
Frostworks 3:dfb4b8d37bab 39 }
Frostworks 3:dfb4b8d37bab 40
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 0:2b420376e01d 73 pc.baud(115200);
Frostworks 0:2b420376e01d 74 pc.printf("hoi\n");
Frostworks 0:2b420376e01d 75 M1_rotate = 1;
Frostworks 1:833c73834749 76 M2_rotate = 1;
Frostworks 1:833c73834749 77 M3_rotate = 1;
Frostworks 2:4fb733fffd1f 78
Frostworks 3:dfb4b8d37bab 79 /*
Frostworks 3:dfb4b8d37bab 80 float GetReferenceVelocity() {
Frostworks 3:dfb4b8d37bab 81 // Returns reference velocity in rad/s.
Frostworks 3:dfb4b8d37bab 82 // Positive value means clockwise rotation.
Frostworks 3:dfb4b8d37bab 83 const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s
Frostworks 3:dfb4b8d37bab 84 if (a) {
Frostworks 3:dfb4b8d37bab 85 // Clockwise rotation
Frostworks 3:dfb4b8d37bab 86 referenceVelocity = potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 87
Frostworks 3:dfb4b8d37bab 88 } else {
Frostworks 3:dfb4b8d37bab 89 // Counterclockwise rotation
Frostworks 3:dfb4b8d37bab 90 referenceVelocity = -1*potMeterIn * maxVelocity;
Frostworks 3:dfb4b8d37bab 91
Frostworks 3:dfb4b8d37bab 92 }
Frostworks 3:dfb4b8d37bab 93 return referenceVelocity;
Frostworks 2:4fb733fffd1f 94 }
Frostworks 2:4fb733fffd1f 95
Frostworks 3:dfb4b8d37bab 96 void SetMotor1(float motorValue) {
Frostworks 3:dfb4b8d37bab 97 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 98 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 99 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 100 // within range
Frostworks 3:dfb4b8d37bab 101 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 102 motor1DirectionPin=1;
Frostworks 3:dfb4b8d37bab 103 } else {
Frostworks 3:dfb4b8d37bab 104 motor1DirectionPin=0;
Frostworks 3:dfb4b8d37bab 105 }
Frostworks 3:dfb4b8d37bab 106 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 107 motor1MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 108 } else {
Frostworks 3:dfb4b8d37bab 109 motor1MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 110 }
Frostworks 2:4fb733fffd1f 111 }
Frostworks 2:4fb733fffd1f 112
Frostworks 3:dfb4b8d37bab 113 void SetMotor2(float motorValue) {
Frostworks 3:dfb4b8d37bab 114 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 115 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 116 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 117 // within range
Frostworks 3:dfb4b8d37bab 118 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 119 motor2DirectionPin=1;
Frostworks 3:dfb4b8d37bab 120 } else {
Frostworks 3:dfb4b8d37bab 121 motor2DirectionPin=0;
Frostworks 3:dfb4b8d37bab 122 }
Frostworks 3:dfb4b8d37bab 123 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 124 motor2MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 125 } else {
Frostworks 3:dfb4b8d37bab 126 motor2MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 127 }
Frostworks 2:4fb733fffd1f 128 }
Frostworks 3:dfb4b8d37bab 129
Frostworks 3:dfb4b8d37bab 130 void SetMotor3(float motorValue) {
Frostworks 3:dfb4b8d37bab 131 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 3:dfb4b8d37bab 132 // bits for motor 1. Positive value makes motor rotating
Frostworks 3:dfb4b8d37bab 133 // clockwise. motorValues outside range are truncated to
Frostworks 3:dfb4b8d37bab 134 // within range
Frostworks 3:dfb4b8d37bab 135 if (motorValue >=0) {
Frostworks 3:dfb4b8d37bab 136 motor3DirectionPin=1;
Frostworks 3:dfb4b8d37bab 137 } else {
Frostworks 3:dfb4b8d37bab 138 motor3DirectionPin=0;
Frostworks 3:dfb4b8d37bab 139 }
Frostworks 3:dfb4b8d37bab 140 if (fabs(motorValue)>1) {
Frostworks 3:dfb4b8d37bab 141 motor3MagnitudePin = 1;
Frostworks 3:dfb4b8d37bab 142 } else {
Frostworks 3:dfb4b8d37bab 143 motor3MagnitudePin = fabs(motorValue);
Frostworks 3:dfb4b8d37bab 144 }
Frostworks 3:dfb4b8d37bab 145 }
Frostworks 3:dfb4b8d37bab 146 */
Frostworks 1:833c73834749 147
Frostworks 0:2b420376e01d 148 while (true) {
Frostworks 2:4fb733fffd1f 149 GetDirections();
Frostworks 3:dfb4b8d37bab 150 if ((draailinks == true) && (draairechts == false)) {
Frostworks 2:4fb733fffd1f 151 M1_Speed = 0.5;
Frostworks 3:dfb4b8d37bab 152 M1_rotate = 0;
Frostworks 3:dfb4b8d37bab 153 } else if ((draairechts == true) && (draailinks == false)) {
Frostworks 2:4fb733fffd1f 154 M1_Speed = 0.5;
Frostworks 3:dfb4b8d37bab 155 M1_rotate = 1;
Frostworks 2:4fb733fffd1f 156 } else {
Frostworks 2:4fb733fffd1f 157 M1_Speed = 0;
Frostworks 1:833c73834749 158 }
Frostworks 2:4fb733fffd1f 159
Frostworks 2:4fb733fffd1f 160
Frostworks 2:4fb733fffd1f 161
Frostworks 3:dfb4b8d37bab 162
Frostworks 2:4fb733fffd1f 163 /*
Frostworks 1:833c73834749 164 float M2 = M2_rotate.read();
Frostworks 1:833c73834749 165 float potje1 = pot1.read();
Frostworks 1:833c73834749 166 float potje2 = pot2.read();
Frostworks 1:833c73834749 167 M1_Speed.write(potje1);
Frostworks 1:833c73834749 168 M3_Speed.write(potje2);
Frostworks 0:2b420376e01d 169 wait(0.1);
Frostworks 1:833c73834749 170 pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
Frostworks 2:4fb733fffd1f 171 */
Frostworks 3:dfb4b8d37bab 172
Frostworks 0:2b420376e01d 173 }
Frostworks 1:833c73834749 174 }