Georgios Ardamerinos / Mbed 2 deprecated DynamicsProject

Phlebot's onboard code

Dependencies:   mbed

Fork of HelloWorld by Simon Ford

main.cpp@8:dc3d3dfc777e, 2017-05-01 (annotated)

Committer:
gardamerinos
Date:
Mon May 01 16:36:48 2017 +0000
Revision:
8:dc3d3dfc777e
Parent:
7:b5053ba85843 
Final Phlebot onboard code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:fb6bbc10ffa0 1 #include "mbed.h"
gardamerinos 4:1070977b83e9 2 #include <vector>
gardamerinos 5:9f6830d1db7b 3 #include <string>
gardamerinos 5:9f6830d1db7b 4 #include <sstream>
gardamerinos 5:9f6830d1db7b 5 #include <iostream>
simon 0:fb6bbc10ffa0 6
gardamerinos 5:9f6830d1db7b 7 //====================ALL DATA========================================
gardamerinos 4:1070977b83e9 8 vector<DigitalOut*> motors;
gardamerinos 5:9f6830d1db7b 9 vector<DigitalOut*> dirs;
gardamerinos 6:e69f8c6faebd 10 vector<DigitalIn*> switches;
gardamerinos 4:1070977b83e9 11 vector<int> steps;
gardamerinos 5:9f6830d1db7b 12 vector<int> currentPosition;
gardamerinos 5:9f6830d1db7b 13 vector<int> goalPosition;
gardamerinos 6:e69f8c6faebd 14 vector<int> buffer;
gardamerinos 7:b5053ba85843 15 bool negativeDirs [4] = {1,1,0,0};
gardamerinos 7:b5053ba85843 16 bool positiveDirs [4] = {0,0,1,1};
gardamerinos 4:1070977b83e9 17
gardamerinos 5:9f6830d1db7b 18 //====================SERIAL COMMUNICATION=============================
gardamerinos 5:9f6830d1db7b 19 Serial pc(USBTX, USBRX);
gardamerinos 5:9f6830d1db7b 20
gardamerinos 5:9f6830d1db7b 21 //===================VARIABLES========================================
gardamerinos 6:e69f8c6faebd 22 int delayus = 200;
gardamerinos 5:9f6830d1db7b 23 int delayusOffset = delayus - 20;
gardamerinos 6:e69f8c6faebd 24 int stepsX = 0;
gardamerinos 6:e69f8c6faebd 25 int stepsY = 0;
gardamerinos 6:e69f8c6faebd 26 int stepsZ = 0;
gardamerinos 6:e69f8c6faebd 27 int stepsRot = 0;
gardamerinos 7:b5053ba85843 28 int mmToSteps = 2400;
gardamerinos 7:b5053ba85843 29 int degToSteps = 4.444;
gardamerinos 6:e69f8c6faebd 30 DigitalOut led(LED1);
gardamerinos 5:9f6830d1db7b 31 //======================FUNCTIONS=======================================
gardamerinos 6:e69f8c6faebd 32 void step(DigitalOut stepsPin,int voltage)
gardamerinos 6:e69f8c6faebd 33 {
gardamerinos 4:1070977b83e9 34 stepsPin = voltage;
gardamerinos 4:1070977b83e9 35 }
simon 0:fb6bbc10ffa0 36
gardamerinos 6:e69f8c6faebd 37 void setDirections(vector<DigitalOut*> dirPin, bool voltage[4])
gardamerinos 6:e69f8c6faebd 38 {
gardamerinos 6:e69f8c6faebd 39 for (int i = 0; i < 4; i++) {
gardamerinos 5:9f6830d1db7b 40 *dirPin[i] = voltage[i];
gardamerinos 5:9f6830d1db7b 41 }
gardamerinos 5:9f6830d1db7b 42 }
gardamerinos 5:9f6830d1db7b 43
gardamerinos 6:e69f8c6faebd 44 void moveOneStep()
gardamerinos 6:e69f8c6faebd 45 {
gardamerinos 6:e69f8c6faebd 46 for (int i = 0; i<motors.size(); i++) {
gardamerinos 6:e69f8c6faebd 47 if (steps.at(i) <= int(0)) {
gardamerinos 6:e69f8c6faebd 48 motors.erase(motors.begin() + i);
gardamerinos 6:e69f8c6faebd 49 steps.erase(steps.begin()+i);
gardamerinos 6:e69f8c6faebd 50 switches.erase(switches.begin()+i);
gardamerinos 4:1070977b83e9 51 }
gardamerinos 6:e69f8c6faebd 52 }
gardamerinos 6:e69f8c6faebd 53 for (int i = 0; i<motors.size(); i++) {
gardamerinos 6:e69f8c6faebd 54 step(*motors[i],1);
gardamerinos 6:e69f8c6faebd 55 }
gardamerinos 6:e69f8c6faebd 56 wait_us(delayus);
gardamerinos 6:e69f8c6faebd 57 for (int i = 0; i<motors.size(); i++) {
gardamerinos 6:e69f8c6faebd 58 step(*motors.at(i),0);
gardamerinos 6:e69f8c6faebd 59 steps.at(i) = steps.at(i)-1;
gardamerinos 6:e69f8c6faebd 60 }
gardamerinos 6:e69f8c6faebd 61 wait_us(delayusOffset);
gardamerinos 5:9f6830d1db7b 62 }
gardamerinos 5:9f6830d1db7b 63
gardamerinos 6:e69f8c6faebd 64 void goHome()
gardamerinos 6:e69f8c6faebd 65 {
gardamerinos 6:e69f8c6faebd 66 while(!(*switches.at(0)) || !(*switches.at(1)) || !(*switches.at(2)) || !(*switches.at(3))) {
gardamerinos 6:e69f8c6faebd 67 for (int i = 0; i < motors.size(); i++) {
gardamerinos 6:e69f8c6faebd 68 if (!(*switches.at(i))) {
gardamerinos 6:e69f8c6faebd 69 steps.at(i)= steps.at(i)+1;
gardamerinos 6:e69f8c6faebd 70 }
gardamerinos 6:e69f8c6faebd 71 }
gardamerinos 6:e69f8c6faebd 72 moveOneStep();
gardamerinos 5:9f6830d1db7b 73 }
gardamerinos 5:9f6830d1db7b 74 }
gardamerinos 6:e69f8c6faebd 75 void goHomeRotation()
gardamerinos 6:e69f8c6faebd 76 {
gardamerinos 6:e69f8c6faebd 77 while(!(*switches.at(3))) {
gardamerinos 6:e69f8c6faebd 78 step(*motors[3],1);
gardamerinos 6:e69f8c6faebd 79 wait_us(8000);
gardamerinos 6:e69f8c6faebd 80 step(*motors.at(3),0);
gardamerinos 6:e69f8c6faebd 81 wait_us(8000);
simon 0:fb6bbc10ffa0 82 }
simon 0:fb6bbc10ffa0 83 }
gardamerinos 5:9f6830d1db7b 84
gardamerinos 6:e69f8c6faebd 85 int millimetersToSteps(int mm){
gardamerinos 6:e69f8c6faebd 86 int steps = mmToSteps*mm;
gardamerinos 6:e69f8c6faebd 87 return steps;
gardamerinos 6:e69f8c6faebd 88 }
gardamerinos 6:e69f8c6faebd 89
gardamerinos 6:e69f8c6faebd 90 int degreesToSteps(int degrees){
gardamerinos 6:e69f8c6faebd 91 int steps = degToSteps*degrees;
gardamerinos 6:e69f8c6faebd 92 return steps;
gardamerinos 6:e69f8c6faebd 93 }
gardamerinos 6:e69f8c6faebd 94
gardamerinos 6:e69f8c6faebd 95 void new_directions(){
gardamerinos 6:e69f8c6faebd 96 bool newDirs [4];
gardamerinos 6:e69f8c6faebd 97 for (int i=0; i<goalPosition.size();i++){
gardamerinos 6:e69f8c6faebd 98 if (goalPosition.at(i) >=0){
gardamerinos 6:e69f8c6faebd 99 newDirs[i] = !negativeDirs[i];
gardamerinos 6:e69f8c6faebd 100 }
gardamerinos 6:e69f8c6faebd 101 else{
gardamerinos 6:e69f8c6faebd 102 newDirs[i] = negativeDirs[i];
gardamerinos 6:e69f8c6faebd 103 }
gardamerinos 6:e69f8c6faebd 104 }
gardamerinos 6:e69f8c6faebd 105 setDirections(dirs, newDirs);
gardamerinos 6:e69f8c6faebd 106 }
gardamerinos 6:e69f8c6faebd 107
gardamerinos 6:e69f8c6faebd 108 void initialization()
gardamerinos 6:e69f8c6faebd 109 {
gardamerinos 5:9f6830d1db7b 110 motors.push_back(new DigitalOut(p21));
gardamerinos 5:9f6830d1db7b 111 motors.push_back(new DigitalOut(p22));
gardamerinos 5:9f6830d1db7b 112 motors.push_back(new DigitalOut(p23));
gardamerinos 5:9f6830d1db7b 113 motors.push_back(new DigitalOut(p24));
gardamerinos 5:9f6830d1db7b 114 steps.push_back(stepsX);
gardamerinos 5:9f6830d1db7b 115 steps.push_back(stepsY);
gardamerinos 5:9f6830d1db7b 116 steps.push_back(stepsZ);
gardamerinos 5:9f6830d1db7b 117 steps.push_back(stepsRot);
gardamerinos 5:9f6830d1db7b 118 dirs.push_back(new DigitalOut(p30));
gardamerinos 5:9f6830d1db7b 119 dirs.push_back(new DigitalOut(p29));
gardamerinos 5:9f6830d1db7b 120 dirs.push_back(new DigitalOut(p28));
gardamerinos 5:9f6830d1db7b 121 dirs.push_back(new DigitalOut(p27));
gardamerinos 6:e69f8c6faebd 122 switches.push_back(new DigitalIn(p15));
gardamerinos 6:e69f8c6faebd 123 switches.push_back(new DigitalIn(p16));
gardamerinos 6:e69f8c6faebd 124 switches.push_back(new DigitalIn(p17));
gardamerinos 6:e69f8c6faebd 125 switches.push_back(new DigitalIn(p18));
gardamerinos 6:e69f8c6faebd 126 switches[0]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 127 switches[1]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 128 switches[2]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 129 switches[3]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 130 }
gardamerinos 6:e69f8c6faebd 131
gardamerinos 6:e69f8c6faebd 132 void re_initialization(){
gardamerinos 7:b5053ba85843 133 motors.clear();
gardamerinos 6:e69f8c6faebd 134 motors.push_back(new DigitalOut(p21));
gardamerinos 6:e69f8c6faebd 135 motors.push_back(new DigitalOut(p22));
gardamerinos 6:e69f8c6faebd 136 motors.push_back(new DigitalOut(p23));
gardamerinos 6:e69f8c6faebd 137 motors.push_back(new DigitalOut(p24));
gardamerinos 7:b5053ba85843 138 steps.clear();
gardamerinos 6:e69f8c6faebd 139 steps.push_back(millimetersToSteps(goalPosition.at(0)));
gardamerinos 6:e69f8c6faebd 140 steps.push_back(millimetersToSteps(goalPosition.at(1)));
gardamerinos 6:e69f8c6faebd 141 steps.push_back(millimetersToSteps(goalPosition.at(2)));
gardamerinos 6:e69f8c6faebd 142 steps.push_back(degreesToSteps(goalPosition.at(3)));
gardamerinos 7:b5053ba85843 143 switches.clear();
gardamerinos 6:e69f8c6faebd 144 switches.push_back(new DigitalIn(p15));
gardamerinos 6:e69f8c6faebd 145 switches.push_back(new DigitalIn(p16));
gardamerinos 6:e69f8c6faebd 146 switches.push_back(new DigitalIn(p17));
gardamerinos 6:e69f8c6faebd 147 switches.push_back(new DigitalIn(p18));
gardamerinos 6:e69f8c6faebd 148 switches[0]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 149 switches[1]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 150 switches[2]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 151 switches[3]->mode(PullUp);
gardamerinos 6:e69f8c6faebd 152 }
gardamerinos 6:e69f8c6faebd 153
gardamerinos 6:e69f8c6faebd 154 void serial_check()
gardamerinos 6:e69f8c6faebd 155 {
gardamerinos 6:e69f8c6faebd 156 if (pc.readable()){
gardamerinos 6:e69f8c6faebd 157 int numberIn;
gardamerinos 7:b5053ba85843 158 // pc.putc(pc.getc());
gardamerinos 6:e69f8c6faebd 159 char *dataStart = (char *)&numberIn;
gardamerinos 6:e69f8c6faebd 160 char byteIn;
gardamerinos 6:e69f8c6faebd 161
gardamerinos 6:e69f8c6faebd 162 for (int i = 0; i < 2; i++) {
gardamerinos 6:e69f8c6faebd 163 byteIn = pc.getc();
gardamerinos 7:b5053ba85843 164 pc.putc(byteIn);
gardamerinos 6:e69f8c6faebd 165 *(dataStart + i) = byteIn;
gardamerinos 6:e69f8c6faebd 166 }
gardamerinos 6:e69f8c6faebd 167 buffer.push_back(numberIn);
gardamerinos 6:e69f8c6faebd 168 if (buffer.size()== 5){
gardamerinos 7:b5053ba85843 169 pc.putc(byteIn);
gardamerinos 6:e69f8c6faebd 170 //state machine at some point
gardamerinos 6:e69f8c6faebd 171 goalPosition.clear();
gardamerinos 6:e69f8c6faebd 172 for (int i = 1; i<5; i++){
gardamerinos 6:e69f8c6faebd 173 goalPosition.push_back(buffer.at(i));
gardamerinos 6:e69f8c6faebd 174 }
gardamerinos 6:e69f8c6faebd 175 re_initialization();
gardamerinos 6:e69f8c6faebd 176 buffer.clear();
gardamerinos 7:b5053ba85843 177 setDirections(dirs, positiveDirs);
gardamerinos 7:b5053ba85843 178 while(motors.size()>0){
gardamerinos 7:b5053ba85843 179 moveOneStep();
gardamerinos 7:b5053ba85843 180 wait_us(1000);
gardamerinos 7:b5053ba85843 181 }
gardamerinos 6:e69f8c6faebd 182 }
gardamerinos 6:e69f8c6faebd 183 }
gardamerinos 5:9f6830d1db7b 184 }
gardamerinos 5:9f6830d1db7b 185
gardamerinos 5:9f6830d1db7b 186 //=================MAIN========================================
gardamerinos 6:e69f8c6faebd 187 int main()
gardamerinos 6:e69f8c6faebd 188 {
gardamerinos 7:b5053ba85843 189 pc.baud(19200);
gardamerinos 5:9f6830d1db7b 190 initialization();
gardamerinos 7:b5053ba85843 191 DigitalOut led(LED1);
gardamerinos 6:e69f8c6faebd 192 setDirections(dirs, negativeDirs);
gardamerinos 6:e69f8c6faebd 193 goHomeRotation();
gardamerinos 6:e69f8c6faebd 194 goHome();
gardamerinos 5:9f6830d1db7b 195 while(1) {
gardamerinos 6:e69f8c6faebd 196 serial_check();
gardamerinos 7:b5053ba85843 197 // led = *switches[1];
gardamerinos 7:b5053ba85843 198 // moveOneStep();
gardamerinos 5:9f6830d1db7b 199 }
gardamerinos 5:9f6830d1db7b 200 }