work but not jet as..
Dependencies: MODSERIAL mbed QEI
Fork of AllMoter_test by
it works for motor 1
main.cpp
- Committer:
- Frostworks
- Date:
- 2016-10-21
- Revision:
- 6:4e4df2f6157e
- Parent:
- 5:353ed56417a2
- Child:
- 7:c7bdeee1bbac
File content as of revision 6:4e4df2f6157e:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" DigitalOut M1_Rotate(D2); // voltage only base rotation PwmOut M1_Speed(D3); // voltage only base rotation MODSERIAL pc(USBTX, USBRX); //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING) QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING); QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING); DigitalOut M2_Rotate(D4); // encoder side pot 2 translation PwmOut M2_Speed(D5); // encoder side pot 2 translation DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation DigitalIn links(SW3); DigitalIn rechts(SW2); //DigitalOut M2_rotate(D6); //PwmOut M2_Speed(D7); AnalogIn pot1(A4); // pot 1 motor 1 AnalogIn pot2(A3); // pot 2 motor 3 bool draairechts; bool draailinks; bool turn = 0; float waiter = 0.1; float afstand = 150; float translation = 0; void GetDirections() { pc.baud(115200); if ((rechts == 0) && (links == 0) && (turn == 0)) { draailinks = 0; draairechts = 0; turn = 1; pc.printf("begin de actie \n \r "); wait(waiter); } else if ((rechts == 0) && (links == 0) && (turn == 1)) { draailinks = 0; draairechts = 0; turn = 0; pc.printf("breek de actie af \n \r "); wait(waiter); } else if ((rechts == 1) && (links == 1)&& (turn == 0)) { } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) { /* if the right button is pressed and the motor isn't rotating to the left, then start rotating to the right etc*/ draairechts = !draairechts; pc.printf("draai naar rechts \n \r "); wait(waiter); } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) { draailinks = 0; draairechts = !draairechts; pc.printf("draai naar rechts na links \n \r "); wait(waiter); } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) { draailinks = !draailinks; pc.printf("draai naar links \n \r "); wait(waiter); } else if ((links == 1) && (draairechts == 1) && (turn == 0)) { draairechts = 0; draailinks = !draailinks; pc.printf("draai naar links na rechts \n \r "); wait(waiter); } wait(2*waiter); } void GetPosition() { float pulses = motor2.getPulses(); float Puls_degree = (8400/360); float degrees = (pulses/Puls_degree); float wheel1 = 16; float wheel2 = 31; float wheel3 = 41; float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1)); float pi = 3.14159265359; float radians = (degrees/360)*2*pi; float translation = ((radians/overbrenging)*32.25); pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses); } void Burgerflip() { M2_Speed = 0.5; M2_Rotate = 1; M3_Speed = 0.5; M3_Rotate = 1; } int main() { while (true) { GetDirections(); if (draairechts == true) { M1_Speed = 0.5; M1_Rotate = 0; } else if (draailinks == true) { M1_Speed = 0.5; M1_Rotate = 1; } else if (turn == 1) { /*M2_Speed = 0.5; M2_Rotate = 1; M3_Speed = 0.5; M3_Rotate = 1;*/ Burgerflip(); } else if (turn == 0) { M2_Speed = 0; M3_Speed = 0; } if ((draailinks == false) && (draairechts == false)) { M1_Speed = 0; } GetPosition(); /* pulses = 8400 */ } } /* float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s if (a) { // Clockwise rotation referenceVelocity = potMeterIn * maxVelocity; } else { // Counterclockwise rotation referenceVelocity = -1*potMeterIn * maxVelocity; } return referenceVelocity; } void SetMotor1(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) { motor1DirectionPin=1; } else { motor1DirectionPin=0; } if (fabs(motorValue)>1) { motor1MagnitudePin = 1; } else { motor1MagnitudePin = fabs(motorValue); } } void SetMotor2(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) { motor2DirectionPin=1; } else { motor2DirectionPin=0; } if (fabs(motorValue)>1) { motor2MagnitudePin = 1; } else { motor2MagnitudePin = fabs(motorValue); } } void SetMotor3(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) { motor3DirectionPin=1; } else { motor3DirectionPin=0; } if (fabs(motorValue)>1) { motor3MagnitudePin = 1; } else { motor3MagnitudePin = fabs(motorValue); } } */