project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@10:18116d979d61, 2017-05-03 (annotated)

Committer:
bjorntukkertje
Date:
Wed May 03 10:12:04 2017 +0000
Revision:
10:18116d979d61
Parent:
8:ec7b7c9ad07e
Child:
11:aac90fca0290
met stepper;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjorntukkertje 0:9756a10feb5f 1 #include "mbed.h"
bjorntukkertje 0:9756a10feb5f 2
bjorntukkertje 0:9756a10feb5f 3 DigitalOut Motor1 (D13);
bjorntukkertje 0:9756a10feb5f 4 DigitalOut Motor2 (A0);
bjorntukkertje 0:9756a10feb5f 5 DigitalOut Motor3 (D12);
bjorntukkertje 0:9756a10feb5f 6 DigitalOut Motor4 (D11);
bjorntukkertje 6:6659a8ada914 7 AnalogIn Sensor1 (A1);
bjorntukkertje 6:6659a8ada914 8 AnalogIn Sensor2 (A2);
bjorntukkertje 6:6659a8ada914 9 AnalogIn Sensor3 (A3);
bjorntukkertje 6:6659a8ada914 10 AnalogIn Sensor4 (A4);
bjorntukkertje 10:18116d979d61 11 DigitalOut steppera (D2);
bjorntukkertje 10:18116d979d61 12 DigitalOut stepperb (D3);
bjorntukkertje 10:18116d979d61 13 DigitalOut stepperc (D4);
bjorntukkertje 10:18116d979d61 14 DigitalOut stepperd (D5);
bjorntukkertje 4:038346651cc0 15
bjorntukkertje 0:9756a10feb5f 16
bjorntukkertje 10:18116d979d61 17 Ticker lidar;
bjorntukkertje 10:18116d979d61 18 Ticker serial;
bjorntukkertje 10:18116d979d61 19
bjorntukkertje 10:18116d979d61 20 Serial pc(USBTX, USBRX);
bjorntukkertje 0:9756a10feb5f 21
bjorntukkertje 10:18116d979d61 22 int stapmode = 0;
bjorntukkertje 10:18116d979d61 23 int position = 0;
bjorntukkertje 10:18116d979d61 24 int pos;
bjorntukkertje 10:18116d979d61 25
bjorntukkertje 10:18116d979d61 26 // poar neemn
bjorntukkertje 10:18116d979d61 27 // twee poar neemn
bjorntukkertje 10:18116d979d61 28 // twee tettn in n envelop
bjorntukkertje 10:18116d979d61 29
bjorntukkertje 10:18116d979d61 30 int stepper(int stapmode)
bjorntukkertje 10:18116d979d61 31 {
bjorntukkertje 10:18116d979d61 32 switch (stapmode) {
bjorntukkertje 10:18116d979d61 33 case 0:
bjorntukkertje 10:18116d979d61 34 steppera = 1;
bjorntukkertje 10:18116d979d61 35 stepperb = 0;
bjorntukkertje 10:18116d979d61 36 stepperc = 1;
bjorntukkertje 10:18116d979d61 37 stepperd = 0;
bjorntukkertje 10:18116d979d61 38 pos ++;
bjorntukkertje 10:18116d979d61 39 break;
bjorntukkertje 10:18116d979d61 40 case 1:
bjorntukkertje 10:18116d979d61 41 steppera = 1;
bjorntukkertje 10:18116d979d61 42 stepperb = 0;
bjorntukkertje 10:18116d979d61 43 stepperc = 0;
bjorntukkertje 10:18116d979d61 44 stepperd = 1;
bjorntukkertje 10:18116d979d61 45 pos ++;
bjorntukkertje 10:18116d979d61 46 break;
bjorntukkertje 10:18116d979d61 47 case 2:
bjorntukkertje 10:18116d979d61 48 steppera = 0;
bjorntukkertje 10:18116d979d61 49 stepperb = 1;
bjorntukkertje 10:18116d979d61 50 stepperc = 0;
bjorntukkertje 10:18116d979d61 51 stepperd = 1;
bjorntukkertje 10:18116d979d61 52 pos ++;
bjorntukkertje 10:18116d979d61 53 break;
bjorntukkertje 10:18116d979d61 54 case 3:
bjorntukkertje 10:18116d979d61 55 steppera = 0;
bjorntukkertje 10:18116d979d61 56 stepperb = 1;
bjorntukkertje 10:18116d979d61 57 stepperc = 1;
bjorntukkertje 10:18116d979d61 58 stepperd = 0;
bjorntukkertje 10:18116d979d61 59 pos ++;
bjorntukkertje 10:18116d979d61 60 break;
bjorntukkertje 10:18116d979d61 61 ;
bjorntukkertje 10:18116d979d61 62 }
bjorntukkertje 10:18116d979d61 63 if (pos > 1000) { //na volledige rotatie ga naar nul
bjorntukkertje 10:18116d979d61 64 pos = 0;
bjorntukkertje 10:18116d979d61 65 }
bjorntukkertje 10:18116d979d61 66 return pos;
bjorntukkertje 10:18116d979d61 67
bjorntukkertje 10:18116d979d61 68 }
bjorntukkertje 10:18116d979d61 69
bjorntukkertje 10:18116d979d61 70 void run_serial() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 71 {
bjorntukkertje 10:18116d979d61 72
bjorntukkertje 10:18116d979d61 73 pc.printf("%d\n",position);
bjorntukkertje 10:18116d979d61 74 }
bjorntukkertje 10:18116d979d61 75
bjorntukkertje 10:18116d979d61 76 void run_lidar() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 77 {
bjorntukkertje 10:18116d979d61 78 stapmode ++;
bjorntukkertje 10:18116d979d61 79 if (stapmode > 3) {
bjorntukkertje 10:18116d979d61 80 stapmode = 0;
bjorntukkertje 10:18116d979d61 81 }
bjorntukkertje 10:18116d979d61 82 position = stepper(stapmode);
bjorntukkertje 10:18116d979d61 83 }
bjorntukkertje 10:18116d979d61 84
bjorntukkertje 8:ec7b7c9ad07e 85
bjorntukkertje 0:9756a10feb5f 86 int main() {
bjorntukkertje 10:18116d979d61 87 lidar.attach(&run_lidar, 0.0001);
bjorntukkertje 10:18116d979d61 88 serial.attach(&run_serial, 0.2);
bjorntukkertje 0:9756a10feb5f 89 while(1) {
bjorntukkertje 8:ec7b7c9ad07e 90 if ( Sensor1 >= 1) {
bjorntukkertje 8:ec7b7c9ad07e 91 Motor1 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 92 Motor2 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 93 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 94 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 95 wait_ms (500);
bjorntukkertje 8:ec7b7c9ad07e 96 Motor1 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 97 Motor2 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 98 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 99 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 100 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 101 }
bjorntukkertje 8:ec7b7c9ad07e 102 if (Sensor2 >= 1) {
bjorntukkertje 6:6659a8ada914 103 Motor1 = 0 ;
bjorntukkertje 6:6659a8ada914 104 Motor2 = 1 ;
bjorntukkertje 6:6659a8ada914 105 Motor3 = 0 ;
bjorntukkertje 6:6659a8ada914 106 Motor4 = 1 ;
bjorntukkertje 6:6659a8ada914 107 wait_ms (500);
bjorntukkertje 6:6659a8ada914 108 Motor1 = 1 ;
bjorntukkertje 6:6659a8ada914 109 Motor2 = 0 ;
bjorntukkertje 6:6659a8ada914 110 Motor3 = 0 ;
bjorntukkertje 6:6659a8ada914 111 Motor4 = 1 ;
bjorntukkertje 6:6659a8ada914 112 wait_ms (1000);
bjorntukkertje 6:6659a8ada914 113 }
bjorntukkertje 8:ec7b7c9ad07e 114 if (Sensor3 >= 1) {
bjorntukkertje 8:ec7b7c9ad07e 115 Motor1 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 116 Motor2 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 117 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 118 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 119 wait_ms (500);
bjorntukkertje 8:ec7b7c9ad07e 120 Motor1 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 121 Motor2 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 122 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 123 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 124 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 125 }
bjorntukkertje 8:ec7b7c9ad07e 126 if (Sensor4 >= 1) {
bjorntukkertje 8:ec7b7c9ad07e 127 Motor1 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 128 Motor2 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 129 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 130 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 131 wait_ms (500);
bjorntukkertje 8:ec7b7c9ad07e 132 Motor1 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 133 Motor2 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 134 Motor3 = 0 ;
bjorntukkertje 8:ec7b7c9ad07e 135 Motor4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 136 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 137 }
bjorntukkertje 2:5c11199e4ff2 138 else {
bjorntukkertje 2:5c11199e4ff2 139 Motor1 = 1 ;
bjorntukkertje 2:5c11199e4ff2 140 Motor2 = 0 ;
bjorntukkertje 2:5c11199e4ff2 141 Motor3 = 1 ;
bjorntukkertje 2:5c11199e4ff2 142 Motor4 = 0 ;
bjorntukkertje 2:5c11199e4ff2 143 }
bjorntukkertje 2:5c11199e4ff2 144
bjorntukkertje 0:9756a10feb5f 145
bjorntukkertje 0:9756a10feb5f 146 }
bjorntukkertje 0:9756a10feb5f 147 }