Georgios Ardamerinos / Mbed 2 deprecated DynamicsProject

Phlebot's onboard code

Dependencies:   mbed

Fork of HelloWorld by Simon Ford

main.cpp@7:b5053ba85843, 2017-04-06 (annotated)

Committer:
gardamerinos
Date:
Thu Apr 06 06:31:20 2017 +0000
Revision:
7:b5053ba85843
Parent:
6:e69f8c6faebd
Child:
8:dc3d3dfc777e
serial communication working, home working

Who changed what in which revision?

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