Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@7:eb239942830d, 2015-09-28 (annotated)

Committer:
Zeekat
Date:
Mon Sep 28 12:25:41 2015 +0000
Revision:
7:eb239942830d
Parent:
6:3031e8abb052
Child:
8:83d522d8f2b8
alles werkend

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:5ea1875b307a 1 #include "mbed.h"
Zeekat 4:e171c9fa5447 2 #include "MODSERIAL.h"
Zeekat 4:e171c9fa5447 3 #include "QEI.h"
Zeekat 4:e171c9fa5447 4
Zeekat 4:e171c9fa5447 5
Zeekat 4:e171c9fa5447 6 Serial pc(USBTX,USBRX);
Zeekat 4:e171c9fa5447 7
Zeekat 4:e171c9fa5447 8 Ticker mod;
Zeekat 0:5ea1875b307a 9
Zeekat 0:5ea1875b307a 10 //MOTOR
Zeekat 0:5ea1875b307a 11 // motor 1 en 2 zijn omgedraait
Zeekat 0:5ea1875b307a 12 // pinnen motor 1 -> pinnen motor 2
Zeekat 0:5ea1875b307a 13
Zeekat 0:5ea1875b307a 14 //motor 1 gegevens
Zeekat 0:5ea1875b307a 15 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 0:5ea1875b307a 16 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:5ea1875b307a 17 // einde motor 1
Zeekat 0:5ea1875b307a 18
Zeekat 0:5ea1875b307a 19 //motor 2 gegevens
Zeekat 0:5ea1875b307a 20 PwmOut motor2_aan(D5); // PWM signaal motor 1 (uit sheets)
Zeekat 0:5ea1875b307a 21 DigitalOut motor2_rich(D4); // digitaal signaal voor richting
Zeekat 0:5ea1875b307a 22 // einde motor 1
Zeekat 0:5ea1875b307a 23 //EINDE MOTOR
Zeekat 0:5ea1875b307a 24
Zeekat 4:e171c9fa5447 25 // ENcoDER
Zeekat 4:e171c9fa5447 26 QEI motor1_enc(D12,D11,NC,64);
Zeekat 4:e171c9fa5447 27 QEI motor2_enc(D10,D9,NC,64);
Zeekat 4:e171c9fa5447 28
Zeekat 0:5ea1875b307a 29
Zeekat 0:5ea1875b307a 30 //POTMETERS
Zeekat 0:5ea1875b307a 31 AnalogIn potleft(A1);
Zeekat 0:5ea1875b307a 32 AnalogIn potright(A0);
Zeekat 0:5ea1875b307a 33
Zeekat 5:46dae55f0ab0 34 DigitalIn button(PTA4);
Zeekat 5:46dae55f0ab0 35 int button_on = 0;
Zeekat 4:e171c9fa5447 36
Zeekat 0:5ea1875b307a 37 void move_mot1(float left)
Zeekat 0:5ea1875b307a 38 {
Zeekat 6:3031e8abb052 39 if(left < 0.400)
Zeekat 1:8918d924f625 40 {
Zeekat 3:c9f9db6581bc 41 float calc1 = left - 1;
Zeekat 3:c9f9db6581bc 42 float calc2 = abs(calc1);
Zeekat 6:3031e8abb052 43 float leftin1 = (calc2-0.6)*2.5 ;
Zeekat 6:3031e8abb052 44 motor1_aan.write(leftin1);
Zeekat 0:5ea1875b307a 45 motor1_rich.write(0);
Zeekat 0:5ea1875b307a 46 }
Zeekat 6:3031e8abb052 47 else if(left > 0.4000 && left < 0.6000)
Zeekat 0:5ea1875b307a 48 {
Zeekat 0:5ea1875b307a 49 motor1_aan.write(0);
Zeekat 0:5ea1875b307a 50 }
Zeekat 6:3031e8abb052 51 else if(left > 0.6000)
Zeekat 0:5ea1875b307a 52 {
Zeekat 6:3031e8abb052 53 float leftin2 = (left-0.6)*2.5;
Zeekat 6:3031e8abb052 54 motor1_aan.write(leftin2);
Zeekat 0:5ea1875b307a 55 motor1_rich.write(1);
Zeekat 0:5ea1875b307a 56 }
Zeekat 0:5ea1875b307a 57 }
Zeekat 0:5ea1875b307a 58
Zeekat 0:5ea1875b307a 59 void move_mot2(float right)
Zeekat 0:5ea1875b307a 60 {
Zeekat 6:3031e8abb052 61 if(right < 0.4000)
Zeekat 0:5ea1875b307a 62 {
Zeekat 3:c9f9db6581bc 63 float calc3 = right - 1;
Zeekat 3:c9f9db6581bc 64 float calc4 = abs(calc3);
Zeekat 6:3031e8abb052 65 float rightin1 = (calc4-0.6)*2.5 ;
Zeekat 6:3031e8abb052 66 motor2_aan.write(rightin1);
Zeekat 0:5ea1875b307a 67 motor2_rich.write(0);
Zeekat 0:5ea1875b307a 68 }
Zeekat 6:3031e8abb052 69 else if(right > 0.4000 && right < 0.6000)
Zeekat 0:5ea1875b307a 70 {
Zeekat 0:5ea1875b307a 71 motor2_aan.write(0);
Zeekat 0:5ea1875b307a 72 }
Zeekat 6:3031e8abb052 73 else if(right > 0.6000)
Zeekat 0:5ea1875b307a 74 {
Zeekat 6:3031e8abb052 75 float rightin2 = (right-0.6)*2.5;
Zeekat 6:3031e8abb052 76 motor2_aan.write(rightin2);
Zeekat 0:5ea1875b307a 77 motor2_rich.write(1);
Zeekat 0:5ea1875b307a 78 }
Zeekat 0:5ea1875b307a 79 }
Zeekat 0:5ea1875b307a 80
Zeekat 0:5ea1875b307a 81
Zeekat 4:e171c9fa5447 82 void send()
Zeekat 4:e171c9fa5447 83 {
Zeekat 4:e171c9fa5447 84 int countsl = motor1_enc.getPulses();
Zeekat 4:e171c9fa5447 85 int countsr = motor2_enc.getPulses();
Zeekat 4:e171c9fa5447 86 pc.printf("motor 1 counts %d motor 2 counts %d \n", countsl, countsr);
Zeekat 4:e171c9fa5447 87 }
Zeekat 4:e171c9fa5447 88
Zeekat 0:5ea1875b307a 89
Zeekat 0:5ea1875b307a 90 int main()
Zeekat 0:5ea1875b307a 91 {
Zeekat 4:e171c9fa5447 92 pc.baud(115200);
Zeekat 4:e171c9fa5447 93 mod.attach(&send,1);
Zeekat 0:5ea1875b307a 94 while(true)
Zeekat 0:5ea1875b307a 95 {
Zeekat 4:e171c9fa5447 96 float left = potleft.read();
Zeekat 4:e171c9fa5447 97 float right = potright.read();
Zeekat 6:3031e8abb052 98 // pc.printf("%f \n",left);
Zeekat 4:e171c9fa5447 99 move_mot1(left);
Zeekat 4:e171c9fa5447 100 move_mot2(right);
Zeekat 7:eb239942830d 101 if(button.read() == button_on)
Zeekat 7:eb239942830d 102 {
Zeekat 7:eb239942830d 103 motor1_enc.reset();
Zeekat 7:eb239942830d 104 motor2_enc.reset();
Zeekat 7:eb239942830d 105 }
Zeekat 7:eb239942830d 106 wait(0.01f);
Zeekat 4:e171c9fa5447 107
Zeekat 0:5ea1875b307a 108 }
Zeekat 0:5ea1875b307a 109 }