K K / Mbed 2 deprecated Movement_code

code for basic movement of robot

Dependencies:   MODSERIAL QEI mbed

main.cpp@14:41d069cf4701, 2015-10-23 (annotated)

Committer:
Technical_Muffin
Date:
Fri Oct 23 09:55:14 2015 +0000
Revision:
14:41d069cf4701
Parent:
13:6cea4a9fcf7a 
Motor movement code with better toggle function, not yet tested. And doesn't contain emg signal part yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Technical_Muffin 0:e2de1fe5b34c 1 #include "mbed.h"
Technical_Muffin 0:e2de1fe5b34c 2 #include "QEI.h"
Technical_Muffin 0:e2de1fe5b34c 3 #include "MODSERIAL.h"
Technical_Muffin 0:e2de1fe5b34c 4 //#include <"math.h">
Technical_Muffin 0:e2de1fe5b34c 5
Technical_Muffin 8:e0fc6dd187a2 6 QEI motor1(D13,D12,NC, 624);//encoder for motor 1
Technical_Muffin 8:e0fc6dd187a2 7 QEI motor2(D11,D10,NC, 624);//encoder for motor 2
Technical_Muffin 0:e2de1fe5b34c 8 MODSERIAL pc(USBTX,USBRX);
Technical_Muffin 8:e0fc6dd187a2 9 DigitalOut direction1(D7);//direction input for motor 1
Technical_Muffin 8:e0fc6dd187a2 10 DigitalOut direction2(D4);//direction input for motor 2
Technical_Muffin 8:e0fc6dd187a2 11 PwmOut speed1(D6);//speed input for motor 1
Technical_Muffin 8:e0fc6dd187a2 12 PwmOut speed2(D5);//speed input for motor 2
Technical_Muffin 14:41d069cf4701 13 InterruptIn button1(PTC6);//test button for starting motor 1 using intteruptin function
Technical_Muffin 14:41d069cf4701 14 InterruptIn button2(PTA4);//test button for starting motor 2 using interruptin function
Technical_Muffin 2:27fe9488ba61 15 DigitalOut led1(LED_RED);
Technical_Muffin 2:27fe9488ba61 16 DigitalOut led2(LED_BLUE);
Technical_Muffin 2:27fe9488ba61 17 DigitalOut led3(LED_GREEN);
Technical_Muffin 14:41d069cf4701 18 bool motor1_dir=0;//set the direction of motor 1
Technical_Muffin 14:41d069cf4701 19 bool motor2_dir = 0;//set the direction of motor 1
Technical_Muffin 14:41d069cf4701 20
Technical_Muffin 14:41d069cf4701 21 void changedirmotor1(){
Technical_Muffin 14:41d069cf4701 22 motor1_dir = !motor1_dir;//code for changing direction of motor 1
Technical_Muffin 14:41d069cf4701 23 }
Technical_Muffin 14:41d069cf4701 24 void changedirmotor2(){
Technical_Muffin 14:41d069cf4701 25 motor2_dir = !motor2_dir;//code for changing direction of motor 2
Technical_Muffin 14:41d069cf4701 26 }
Technical_Muffin 14:41d069cf4701 27
Technical_Muffin 0:e2de1fe5b34c 28
Technical_Muffin 0:e2de1fe5b34c 29 int main()
Technical_Muffin 3:b913f8ea69e8 30 {
Technical_Muffin 12:2d3d7a9ca496 31 float cycle = 0.3;//define the speed of the motor
Technical_Muffin 8:e0fc6dd187a2 32 bool motor1_on = 1;//set the on variable of motor 1
Technical_Muffin 14:41d069cf4701 33
Technical_Muffin 8:e0fc6dd187a2 34 bool motor2_on =1;//set the on variable of motor 2
Technical_Muffin 12:2d3d7a9ca496 35 int n1=1;//numeric condtions to determine if the speed needs to be increased
Technical_Muffin 11:be7660614c5c 36 int n2=1;
Technical_Muffin 12:2d3d7a9ca496 37 bool CW =1;
Technical_Muffin 12:2d3d7a9ca496 38 bool CCW = 0;
Technical_Muffin 14:41d069cf4701 39 button1.mode(PullDown);//setting the modes for the way the button signals are registered
Technical_Muffin 14:41d069cf4701 40 button2.mode(PullDown);
Technical_Muffin 14:41d069cf4701 41
Technical_Muffin 3:b913f8ea69e8 42 while(1){
Technical_Muffin 3:b913f8ea69e8 43 led3.write(0);
Technical_Muffin 3:b913f8ea69e8 44 led1.write(1);
Technical_Muffin 3:b913f8ea69e8 45 led2.write(1);
Technical_Muffin 8:e0fc6dd187a2 46 pc.baud(115200);
Technical_Muffin 8:e0fc6dd187a2 47
Technical_Muffin 14:41d069cf4701 48 button1.fall(changedirmotor1);//replacement code for toggle function, thus changing direction when pressed
Technical_Muffin 14:41d069cf4701 49 /*
Technical_Muffin 13:6cea4a9fcf7a 50 int diffa1 = -button1.read();//read out the button 1 signal and calculate if it is being pressed or released interrupt function might work better as toggle
Technical_Muffin 13:6cea4a9fcf7a 51 wait(0.3);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 52 int diffb1 = button1.read();
Technical_Muffin 8:e0fc6dd187a2 53 int button_toggle1 = diffa1-diffb1;
Technical_Muffin 12:2d3d7a9ca496 54 if(button_toggle1 == 1 && motor1_dir == CW){
Technical_Muffin 12:2d3d7a9ca496 55 motor1_dir = CCW;
Technical_Muffin 7:46d9f01afba2 56 }
Technical_Muffin 12:2d3d7a9ca496 57 else if(button_toggle1 == 1 && motor1_dir == CCW){
Technical_Muffin 12:2d3d7a9ca496 58 motor1_dir = CW;
Technical_Muffin 7:46d9f01afba2 59 }
Technical_Muffin 14:41d069cf4701 60 */
Technical_Muffin 12:2d3d7a9ca496 61 if(button1.read() !=motor1_on){//check if button 1 is pressed
Technical_Muffin 11:be7660614c5c 62 led3.write(1);
Technical_Muffin 11:be7660614c5c 63 led1.write(0);
Technical_Muffin 12:2d3d7a9ca496 64 while(n1 == 1){
Technical_Muffin 12:2d3d7a9ca496 65 speed1.write(cycle);//write speed only on first run through the loop
Technical_Muffin 12:2d3d7a9ca496 66 direction1.write(motor1_dir);//turn motor CCW or CW
Technical_Muffin 12:2d3d7a9ca496 67
Technical_Muffin 11:be7660614c5c 68 n1=0;
Technical_Muffin 11:be7660614c5c 69 }
Technical_Muffin 11:be7660614c5c 70 }
Technical_Muffin 12:2d3d7a9ca496 71 else if (button1.read() == motor1_on){//when button is released
Technical_Muffin 12:2d3d7a9ca496 72 while(n1==0){//check if the first run was done
Technical_Muffin 12:2d3d7a9ca496 73 speed1.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 11:be7660614c5c 74 n1=1;
Technical_Muffin 11:be7660614c5c 75 }
Technical_Muffin 11:be7660614c5c 76 }
Technical_Muffin 14:41d069cf4701 77 button2.fall(changedirmotor2);//replacement code for toggle function, thus changing direction when pressed
Technical_Muffin 14:41d069cf4701 78
Technical_Muffin 14:41d069cf4701 79 /*
Technical_Muffin 8:e0fc6dd187a2 80 int diffa2 = button2.read();//read out the button 2 signal and calculate if it is being pressed or released
Technical_Muffin 13:6cea4a9fcf7a 81 wait(0.3);//from this we can determine if the rotation direction needs to be reversed.
Technical_Muffin 8:e0fc6dd187a2 82 int diffb2 = button2.read();
Technical_Muffin 8:e0fc6dd187a2 83 int button_toggle2 = diffa2-diffb2;
Technical_Muffin 12:2d3d7a9ca496 84 if(button_toggle2 == 1 && motor2_dir == CW){
Technical_Muffin 12:2d3d7a9ca496 85 motor2_dir = CCW;
Technical_Muffin 8:e0fc6dd187a2 86 }
Technical_Muffin 12:2d3d7a9ca496 87 else if(button_toggle2 == 1 && motor2_dir == CCW){
Technical_Muffin 12:2d3d7a9ca496 88 motor2_dir = CW;
Technical_Muffin 8:e0fc6dd187a2 89 }
Technical_Muffin 14:41d069cf4701 90 */
Technical_Muffin 14:41d069cf4701 91
Technical_Muffin 12:2d3d7a9ca496 92 if(button2.read()!=motor2_on){//check if button 2 is pressed
Technical_Muffin 11:be7660614c5c 93 led3.write(1);
Technical_Muffin 11:be7660614c5c 94 led2.write(0);
Technical_Muffin 12:2d3d7a9ca496 95 while(n2 == 1){
Technical_Muffin 12:2d3d7a9ca496 96 speed2.write(cycle);//write speed only on first run through the loop
Technical_Muffin 13:6cea4a9fcf7a 97 direction2.write(motor2_dir);//turn motor CCW or CW
Technical_Muffin 12:2d3d7a9ca496 98
Technical_Muffin 11:be7660614c5c 99 n2=0;
Technical_Muffin 11:be7660614c5c 100 }
Technical_Muffin 11:be7660614c5c 101 }
Technical_Muffin 12:2d3d7a9ca496 102 else if (button2.read() == motor2_on){//when button is released
Technical_Muffin 12:2d3d7a9ca496 103 while(n2==0){//check if the first run was done
Technical_Muffin 12:2d3d7a9ca496 104 speed2.write(0);//if so set speed to 0 and reset the run counter
Technical_Muffin 11:be7660614c5c 105 n2=1;
Technical_Muffin 11:be7660614c5c 106 }
Technical_Muffin 12:2d3d7a9ca496 107 }
Technical_Muffin 3:b913f8ea69e8 108 }
Technical_Muffin 8:e0fc6dd187a2 109 }