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@5:353ed56417a2, 2016-10-17 (annotated)

Committer:
Frostworks
Date:
Mon Oct 17 12:41:39 2016 +0000
Revision:
5:353ed56417a2
Parent:
4:7b8bf798bb72
Child:
6:4e4df2f6157e
it works hello;

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