Austin Hague / Mbed 2 deprecated Project_MechatronBot

Mechatron Bot

Dependencies:   Motor PinDetect Servo mbed

main.cpp@0:63da85966b5c, 2015-10-27 (annotated)

Committer:
m182376
Date:
Tue Oct 27 01:30:20 2015 +0000
Revision:
0:63da85966b5c
Final Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m182376 0:63da85966b5c 1 #include "mbed.h"
m182376 0:63da85966b5c 2 #include "Motor.h"
m182376 0:63da85966b5c 3 #include "Servo.h"
m182376 0:63da85966b5c 4 #include "PinDetect.h"
m182376 0:63da85966b5c 5
m182376 0:63da85966b5c 6 Motor m(p25, p27, p28);
m182376 0:63da85966b5c 7
m182376 0:63da85966b5c 8 DigitalIn sw1(p16);
m182376 0:63da85966b5c 9 DigitalIn sw2(p17);
m182376 0:63da85966b5c 10 DigitalIn sw3(p18);
m182376 0:63da85966b5c 11 DigitalIn sw4(p19);
m182376 0:63da85966b5c 12 DigitalIn sw5(p20);
m182376 0:63da85966b5c 13 PinDetect push1(p12);
m182376 0:63da85966b5c 14 PinDetect push2(p13);
m182376 0:63da85966b5c 15 Servo servo1(p22);
m182376 0:63da85966b5c 16 Servo servo2(p21);
m182376 0:63da85966b5c 17
m182376 0:63da85966b5c 18 BusOut lights(p5, p6, p7, p8, p11);
m182376 0:63da85966b5c 19
m182376 0:63da85966b5c 20 float a;
m182376 0:63da85966b5c 21 float b;
m182376 0:63da85966b5c 22
m182376 0:63da85966b5c 23 int volatile count=0;
m182376 0:63da85966b5c 24
m182376 0:63da85966b5c 25 // Callback routine is interrupt activated by a debounced pb1 hit
m182376 0:63da85966b5c 26 void push1_hit_callback (void)
m182376 0:63da85966b5c 27 {
m182376 0:63da85966b5c 28
m182376 0:63da85966b5c 29 if (a > 0) {
m182376 0:63da85966b5c 30
m182376 0:63da85966b5c 31 servo1 = 0;
m182376 0:63da85966b5c 32 wait(.2);
m182376 0:63da85966b5c 33 servo1 = 0.5;
m182376 0:63da85966b5c 34
m182376 0:63da85966b5c 35 } else if (a < 0) {
m182376 0:63da85966b5c 36
m182376 0:63da85966b5c 37 servo1 = 1;
m182376 0:63da85966b5c 38 wait(.2);
m182376 0:63da85966b5c 39 servo1 = 0.5;
m182376 0:63da85966b5c 40
m182376 0:63da85966b5c 41 }
m182376 0:63da85966b5c 42
m182376 0:63da85966b5c 43 }
m182376 0:63da85966b5c 44 // Callback routine is interrupt activated by a debounced pb2 hit
m182376 0:63da85966b5c 45 void push2_hit_callback (void)
m182376 0:63da85966b5c 46 {
m182376 0:63da85966b5c 47
m182376 0:63da85966b5c 48 if (a > 0) {
m182376 0:63da85966b5c 49
m182376 0:63da85966b5c 50 servo2 = 0;
m182376 0:63da85966b5c 51 wait(.2);
m182376 0:63da85966b5c 52 servo2 = 0.5;
m182376 0:63da85966b5c 53
m182376 0:63da85966b5c 54 }
m182376 0:63da85966b5c 55
m182376 0:63da85966b5c 56 else if (a < 0) {
m182376 0:63da85966b5c 57
m182376 0:63da85966b5c 58 servo2 = 1;
m182376 0:63da85966b5c 59 wait(.2);
m182376 0:63da85966b5c 60 servo2 = 0.5;
m182376 0:63da85966b5c 61 }
m182376 0:63da85966b5c 62
m182376 0:63da85966b5c 63 }
m182376 0:63da85966b5c 64 int main()
m182376 0:63da85966b5c 65 {
m182376 0:63da85966b5c 66
m182376 0:63da85966b5c 67 servo1 = 0.5;
m182376 0:63da85966b5c 68 servo2 = 0.5;
m182376 0:63da85966b5c 69
m182376 0:63da85966b5c 70 // Use internal pullups for pushbutton
m182376 0:63da85966b5c 71 push1.mode(PullUp);
m182376 0:63da85966b5c 72 push2.mode(PullUp);
m182376 0:63da85966b5c 73 // Delay for initial pullup to take effect
m182376 0:63da85966b5c 74 wait(.01);
m182376 0:63da85966b5c 75 // Setup Interrupt callback functions for a pb hit
m182376 0:63da85966b5c 76 push1.attach_deasserted(&push1_hit_callback);
m182376 0:63da85966b5c 77 push2.attach_deasserted(&push2_hit_callback);
m182376 0:63da85966b5c 78 // Start sampling pb inputs using interrupts
m182376 0:63da85966b5c 79 push1.setSampleFrequency();
m182376 0:63da85966b5c 80 push2.setSampleFrequency();
m182376 0:63da85966b5c 81 //Blink myled in main routine forever while responding to pb changes
m182376 0:63da85966b5c 82 // via interrupts that activate the callback counter function
m182376 0:63da85966b5c 83
m182376 0:63da85966b5c 84 servo1 = 0.5;
m182376 0:63da85966b5c 85 servo2 = 0.5;
m182376 0:63da85966b5c 86 servo1.calibrate (.0009, 90);
m182376 0:63da85966b5c 87 servo2.calibrate (.0009, 90);
m182376 0:63da85966b5c 88
m182376 0:63da85966b5c 89 while(1) {
m182376 0:63da85966b5c 90
m182376 0:63da85966b5c 91 a = rand()/(float)RAND_MAX*(-1.0)+0.4;
m182376 0:63da85966b5c 92 b = rand()/(float)RAND_MAX*5.0 +3.0;
m182376 0:63da85966b5c 93
m182376 0:63da85966b5c 94 if(sw1 == 1 && sw2!= 1 && sw3 != 1 && sw4 != 1) { //player1
m182376 0:63da85966b5c 95
m182376 0:63da85966b5c 96
m182376 0:63da85966b5c 97
m182376 0:63da85966b5c 98 if(a>=.28 || a <= -.28) {
m182376 0:63da85966b5c 99
m182376 0:63da85966b5c 100 servo2 = 0;
m182376 0:63da85966b5c 101
m182376 0:63da85966b5c 102 lights = 31; // LED 1 on
m182376 0:63da85966b5c 103 wait(.125); // Wait .4 seconds
m182376 0:63da85966b5c 104 lights= 0;
m182376 0:63da85966b5c 105
m182376 0:63da85966b5c 106 m.speed(a);
m182376 0:63da85966b5c 107 wait(b);
m182376 0:63da85966b5c 108
m182376 0:63da85966b5c 109
m182376 0:63da85966b5c 110
m182376 0:63da85966b5c 111 }
m182376 0:63da85966b5c 112 } else if(sw1 != 1 && sw2== 1 && sw3 != 1 && sw4 != 1) { //player2
m182376 0:63da85966b5c 113 if(a>=.28 || a <= -.28) {
m182376 0:63da85966b5c 114 lights = 31; //
m182376 0:63da85966b5c 115 wait(.125); //
m182376 0:63da85966b5c 116 lights = 0; //
m182376 0:63da85966b5c 117
m182376 0:63da85966b5c 118 m.speed(a);
m182376 0:63da85966b5c 119 wait(b);
m182376 0:63da85966b5c 120
m182376 0:63da85966b5c 121 }
m182376 0:63da85966b5c 122 } else if(sw1 != 1 && sw2!= 1 && sw3 == 1 && sw4 != 1 && sw5 != 1) { //player3
m182376 0:63da85966b5c 123 if(a>=.28 ) {
m182376 0:63da85966b5c 124
m182376 0:63da85966b5c 125 lights = 31; // LED 1&2&3&4&5 on
m182376 0:63da85966b5c 126 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 127 lights = 0; //LED 1&2&3&4 on
m182376 0:63da85966b5c 128
m182376 0:63da85966b5c 129 m.speed(a);
m182376 0:63da85966b5c 130 wait(b);
m182376 0:63da85966b5c 131
m182376 0:63da85966b5c 132 }
m182376 0:63da85966b5c 133 } else if(sw1 != 1 && sw2!= 1 && sw3 == 1 && sw5 ==1 && sw4 != 1) { //player3
m182376 0:63da85966b5c 134 if(a>=.28 ) {
m182376 0:63da85966b5c 135
m182376 0:63da85966b5c 136 lights = 31; // LED 1&2&3&4&5 on
m182376 0:63da85966b5c 137 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 138 lights = 0; //LED 1&2&3&4 on
m182376 0:63da85966b5c 139
m182376 0:63da85966b5c 140 m.speed(-a);
m182376 0:63da85966b5c 141 wait(b);
m182376 0:63da85966b5c 142
m182376 0:63da85966b5c 143 }
m182376 0:63da85966b5c 144 } else if(sw4 == 1 && sw1 != 1 && sw2!= 1 && sw3 == 1 && sw5 ==1 || sw4 == 1 && sw1 != 1 && sw2!= 1 && sw3 == 1 || sw4 == 1 && sw1 != 1 && sw2== 1 && sw3 != 1 || sw4 == 1 && sw1 == 1 && sw2!= 1 && sw3 != 1 || sw4 == 1 && sw1 != 1 && sw2!= 1 && sw3 != 1 ) { //player3
m182376 0:63da85966b5c 145
m182376 0:63da85966b5c 146 m.speed(0);
m182376 0:63da85966b5c 147 lights = 31; // LED 1&2&3 on
m182376 0:63da85966b5c 148 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 149 lights = 0; //LED 1&2 on
m182376 0:63da85966b5c 150 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 151 lights = 31; //LED 1 on
m182376 0:63da85966b5c 152 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 153 lights = 0; //LED 1&2 on
m182376 0:63da85966b5c 154 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 155 lights = 31; //LED 1 on
m182376 0:63da85966b5c 156 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 157 lights = 0; //LED 1&2 on
m182376 0:63da85966b5c 158 wait(.125); //Wait .4 seconds
m182376 0:63da85966b5c 159
m182376 0:63da85966b5c 160 }
m182376 0:63da85966b5c 161 }
m182376 0:63da85966b5c 162 }