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

Committer:
Frostworks
Date:
Fri Oct 14 09:03:35 2016 +0000
Revision:
2:4fb733fffd1f
Parent:
1:833c73834749
Child:
3:dfb4b8d37bab
aangepast met bools for directions

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 0:2b420376e01d 22
Frostworks 0:2b420376e01d 23 int main()
Frostworks 0:2b420376e01d 24 {
Frostworks 0:2b420376e01d 25 pc.baud(115200);
Frostworks 0:2b420376e01d 26 pc.printf("hoi\n");
Frostworks 0:2b420376e01d 27 M1_rotate = 1;
Frostworks 1:833c73834749 28 M2_rotate = 1;
Frostworks 1:833c73834749 29 M3_rotate = 1;
Frostworks 2:4fb733fffd1f 30 a = 1;
Frostworks 2:4fb733fffd1f 31
Frostworks 2:4fb733fffd1f 32 float GetReferenceVelocity() {
Frostworks 2:4fb733fffd1f 33 // Returns reference velocity in rad/s.
Frostworks 2:4fb733fffd1f 34 // Positive value means clockwise rotation.
Frostworks 2:4fb733fffd1f 35 const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s
Frostworks 2:4fb733fffd1f 36 if (a) {
Frostworks 2:4fb733fffd1f 37 // Clockwise rotation
Frostworks 2:4fb733fffd1f 38 referenceVelocity = potMeterIn * maxVelocity;
Frostworks 2:4fb733fffd1f 39
Frostworks 2:4fb733fffd1f 40 } else {
Frostworks 2:4fb733fffd1f 41 // Counterclockwise rotation
Frostworks 2:4fb733fffd1f 42 referenceVelocity = -1*potMeterIn * maxVelocity;
Frostworks 2:4fb733fffd1f 43
Frostworks 2:4fb733fffd1f 44 }
Frostworks 2:4fb733fffd1f 45 return referenceVelocity;
Frostworks 2:4fb733fffd1f 46 }
Frostworks 2:4fb733fffd1f 47
Frostworks 2:4fb733fffd1f 48 void SetMotor1(float motorValue) {
Frostworks 2:4fb733fffd1f 49 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 2:4fb733fffd1f 50 // bits for motor 1. Positive value makes motor rotating
Frostworks 2:4fb733fffd1f 51 // clockwise. motorValues outside range are truncated to
Frostworks 2:4fb733fffd1f 52 // within range
Frostworks 2:4fb733fffd1f 53 if (motorValue >=0) {
Frostworks 2:4fb733fffd1f 54 motor1DirectionPin=1;
Frostworks 2:4fb733fffd1f 55 } else {
Frostworks 2:4fb733fffd1f 56 motor1DirectionPin=0;
Frostworks 2:4fb733fffd1f 57 }
Frostworks 2:4fb733fffd1f 58 if (fabs(motorValue)>1) {
Frostworks 2:4fb733fffd1f 59 motor1MagnitudePin = 1;
Frostworks 2:4fb733fffd1f 60 } else {
Frostworks 2:4fb733fffd1f 61 motor1MagnitudePin = fabs(motorValue);
Frostworks 2:4fb733fffd1f 62 }
Frostworks 2:4fb733fffd1f 63 }
Frostworks 2:4fb733fffd1f 64
Frostworks 2:4fb733fffd1f 65 void SetMotor2(float motorValue) {
Frostworks 2:4fb733fffd1f 66 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 2:4fb733fffd1f 67 // bits for motor 1. Positive value makes motor rotating
Frostworks 2:4fb733fffd1f 68 // clockwise. motorValues outside range are truncated to
Frostworks 2:4fb733fffd1f 69 // within range
Frostworks 2:4fb733fffd1f 70 if (motorValue >=0) {
Frostworks 2:4fb733fffd1f 71 motor2DirectionPin=1;
Frostworks 2:4fb733fffd1f 72 } else {
Frostworks 2:4fb733fffd1f 73 motor2DirectionPin=0;
Frostworks 2:4fb733fffd1f 74 }
Frostworks 2:4fb733fffd1f 75 if (fabs(motorValue)>1) {
Frostworks 2:4fb733fffd1f 76 motor2MagnitudePin = 1;
Frostworks 2:4fb733fffd1f 77 } else {
Frostworks 2:4fb733fffd1f 78 motor2MagnitudePin = fabs(motorValue);
Frostworks 2:4fb733fffd1f 79 }
Frostworks 2:4fb733fffd1f 80 }
Frostworks 2:4fb733fffd1f 81
Frostworks 2:4fb733fffd1f 82 void SetMotor3(float motorValue) {
Frostworks 2:4fb733fffd1f 83 // Given -1<=motorValue<=1, this sets the PWM and direction
Frostworks 2:4fb733fffd1f 84 // bits for motor 1. Positive value makes motor rotating
Frostworks 2:4fb733fffd1f 85 // clockwise. motorValues outside range are truncated to
Frostworks 2:4fb733fffd1f 86 // within range
Frostworks 2:4fb733fffd1f 87 if (motorValue >=0) {
Frostworks 2:4fb733fffd1f 88 motor3DirectionPin=1;
Frostworks 2:4fb733fffd1f 89 } else {
Frostworks 2:4fb733fffd1f 90 motor3DirectionPin=0;
Frostworks 2:4fb733fffd1f 91 }
Frostworks 2:4fb733fffd1f 92 if (fabs(motorValue)>1) {
Frostworks 2:4fb733fffd1f 93 motor3MagnitudePin = 1;
Frostworks 2:4fb733fffd1f 94 } else {
Frostworks 2:4fb733fffd1f 95 motor3MagnitudePin = fabs(motorValue);
Frostworks 2:4fb733fffd1f 96 }
Frostworks 2:4fb733fffd1f 97 }
Frostworks 2:4fb733fffd1f 98 bool GetDirections() {
Frostworks 2:4fb733fffd1f 99 bool draairechts = false;
Frostworks 2:4fb733fffd1f 100 bool draailinks = false;
Frostworks 2:4fb733fffd1f 101 if (rechts == 1 && draairechts = false) { //Als je rechts aanspant en hij draait nog niet naar rechts
Frostworks 2:4fb733fffd1f 102 draairechts = true;
Frostworks 2:4fb733fffd1f 103 } else if (rechts == 1 && draairechts = true) { //Als je rechts aanspant en hij draait al naar rechts
Frostworks 2:4fb733fffd1f 104 draairechts = false;
Frostworks 2:4fb733fffd1f 105 } else if (links == 1 && draailinks = false) { //Als je links aanspant en hij draait nog niet naar links
Frostworks 2:4fb733fffd1f 106 draailinks = true;
Frostworks 2:4fb733fffd1f 107 } else if (links == 1 && draailinks = true) { // Als je links aanspant en hij draait al naar links
Frostworks 2:4fb733fffd1f 108 draailinks = false;
Frostworks 2:4fb733fffd1f 109 }
Frostworks 2:4fb733fffd1f 110
Frostworks 2:4fb733fffd1f 111 else if (links == 1 && draairechts = true) { // Als je links aanspant en hij draait nog naar rechts
Frostworks 2:4fb733fffd1f 112 draailinks = true;
Frostworks 2:4fb733fffd1f 113 draairechts = false;
Frostworks 2:4fb733fffd1f 114 } else if (rechts == 1 && draailinks = true) { // Als je rechts aanspant en hij draait nog naar links
Frostworks 2:4fb733fffd1f 115 draairechts = true;
Frostworks 2:4fb733fffd1f 116 draailinks = false;
Frostworks 2:4fb733fffd1f 117 }
Frostworks 2:4fb733fffd1f 118 return draailinks;
Frostworks 2:4fb733fffd1f 119 return draairechts;
Frostworks 2:4fb733fffd1f 120 }
Frostworks 1:833c73834749 121
Frostworks 0:2b420376e01d 122 while (true) {
Frostworks 2:4fb733fffd1f 123 GetDirections();
Frostworks 2:4fb733fffd1f 124 if (draailinks == true) {
Frostworks 2:4fb733fffd1f 125 M1_Speed = 0.5;
Frostworks 2:4fb733fffd1f 126 M1_Rotate = 0;
Frostworks 2:4fb733fffd1f 127 } else if (draairechts == true) {
Frostworks 2:4fb733fffd1f 128 M1_Speed = 0.5;
Frostworks 2:4fb733fffd1f 129 M1_Rotate = 1;
Frostworks 2:4fb733fffd1f 130 } else {
Frostworks 2:4fb733fffd1f 131 M1_Speed = 0;
Frostworks 1:833c73834749 132 }
Frostworks 2:4fb733fffd1f 133
Frostworks 2:4fb733fffd1f 134
Frostworks 2:4fb733fffd1f 135
Frostworks 2:4fb733fffd1f 136 /*
Frostworks 1:833c73834749 137 float M2 = M2_rotate.read();
Frostworks 1:833c73834749 138 float potje1 = pot1.read();
Frostworks 1:833c73834749 139 float potje2 = pot2.read();
Frostworks 1:833c73834749 140 M1_Speed.write(potje1);
Frostworks 1:833c73834749 141 M3_Speed.write(potje2);
Frostworks 0:2b420376e01d 142 wait(0.1);
Frostworks 1:833c73834749 143 pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
Frostworks 2:4fb733fffd1f 144 */
Frostworks 2:4fb733fffd1f 145
Frostworks 0:2b420376e01d 146 }
Frostworks 1:833c73834749 147 }