Robotics Studio / Mbed 2 deprecated NucleoBoard_1

Board1

Dependencies:   mbed Map

main.cpp@2:2ef793d0ac01, 2019-04-20 (annotated)

Committer:
choyai
Date:
Sat Apr 20 09:35:37 2019 +0000
Revision:
2:2ef793d0ac01
Parent:
1:4eeae0f92c4c 
buggy version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:97879c8efe59 1 //Module8-9
pbdt1997 0:97879c8efe59 2 //Nucleo Board 1 (Master)
pbdt1997 0:97879c8efe59 3
pbdt1997 0:97879c8efe59 4 #include "mbed.h"
pbdt1997 0:97879c8efe59 5 #include <Map.hpp>
pbdt1997 0:97879c8efe59 6
pbdt1997 0:97879c8efe59 7 Serial pc(USBTX, USBRX);
pbdt1997 0:97879c8efe59 8
pbdt1997 0:97879c8efe59 9 SPI master(PA_7, PA_6, PA_5); //SPI1_MOSI, SPI1_MISO, SPI1_SCLK
pbdt1997 1:4eeae0f92c4c 10 SPI mstrEnc2(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
pbdt1997 1:4eeae0f92c4c 11 SPI mstrEnc1(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
pbdt1997 0:97879c8efe59 12
pbdt1997 0:97879c8efe59 13 DigitalOut csMstr(PA_15); //SPI1_SSEL Master
pbdt1997 1:4eeae0f92c4c 14 DigitalOut csEnc2(PB_12); //SPI2_SSEL Encoder 1
pbdt1997 1:4eeae0f92c4c 15 DigitalOut csEnc1(PA_4); //SPI3_SSEL Encoder 2
pbdt1997 0:97879c8efe59 16
choyai 2:2ef793d0ac01 17 Ticker encTicker;
choyai 2:2ef793d0ac01 18
choyai 2:2ef793d0ac01 19
pbdt1997 1:4eeae0f92c4c 20 DigitalOut dir1(PC_0);
pbdt1997 1:4eeae0f92c4c 21 PwmOut pwm1(PC_9);
pbdt1997 1:4eeae0f92c4c 22
pbdt1997 1:4eeae0f92c4c 23 DigitalOut dir2(PC_1);
pbdt1997 1:4eeae0f92c4c 24 PwmOut pwm2(PC_8);
pbdt1997 0:97879c8efe59 25
pbdt1997 0:97879c8efe59 26 //Parameters
pbdt1997 0:97879c8efe59 27
pbdt1997 0:97879c8efe59 28 char array[4];
pbdt1997 0:97879c8efe59 29 char arraySize = sizeof(array);
pbdt1997 0:97879c8efe59 30 char getData = 0;
pbdt1997 0:97879c8efe59 31 char checkData = 0;
pbdt1997 0:97879c8efe59 32 char encArr[2]= {255,255};
pbdt1997 0:97879c8efe59 33 char encLength = 2;
pbdt1997 0:97879c8efe59 34 bool start = false;
choyai 2:2ef793d0ac01 35 float q1 = 0;
choyai 2:2ef793d0ac01 36 float q2 = 0;
pbdt1997 0:97879c8efe59 37
pbdt1997 1:4eeae0f92c4c 38
pbdt1997 0:97879c8efe59 39 /////////////////////////////
pbdt1997 0:97879c8efe59 40
pbdt1997 0:97879c8efe59 41 //Functions
pbdt1997 0:97879c8efe59 42
pbdt1997 1:4eeae0f92c4c 43 //store data from pc
pbdt1997 0:97879c8efe59 44 void RX(int data){
pbdt1997 0:97879c8efe59 45 array[checkData] = data;
pbdt1997 0:97879c8efe59 46 checkData++;
pbdt1997 0:97879c8efe59 47 if(checkData == arraySize){
pbdt1997 0:97879c8efe59 48 getData = 1;
pbdt1997 0:97879c8efe59 49 checkData = 0;
pbdt1997 0:97879c8efe59 50 }
pbdt1997 0:97879c8efe59 51 }
pbdt1997 0:97879c8efe59 52
pbdt1997 1:4eeae0f92c4c 53 //receive data from pc
pbdt1997 0:97879c8efe59 54 void RX_INT(){
pbdt1997 0:97879c8efe59 55 int data = pc.getc();
pbdt1997 0:97879c8efe59 56 RX(data);
pbdt1997 0:97879c8efe59 57 }
pbdt1997 0:97879c8efe59 58
pbdt1997 1:4eeae0f92c4c 59 //convert 14bit data to angle
pbdt1997 0:97879c8efe59 60 float ConvertAngle(int encData){
pbdt1997 0:97879c8efe59 61 Map map(0, 16383, 0, 359);
pbdt1997 0:97879c8efe59 62 float angle = map.Calculate(encData);
pbdt1997 0:97879c8efe59 63 return angle;
pbdt1997 0:97879c8efe59 64 }
pbdt1997 0:97879c8efe59 65
pbdt1997 1:4eeae0f92c4c 66 //read encoder of joint 1
pbdt1997 0:97879c8efe59 67 float readEnc1(){
pbdt1997 0:97879c8efe59 68 csEnc1 = 0;
pbdt1997 0:97879c8efe59 69 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 70 int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 71 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 72 csEnc1 = 1;
pbdt1997 0:97879c8efe59 73 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 74 }
pbdt1997 0:97879c8efe59 75
pbdt1997 1:4eeae0f92c4c 76 //read encoder of joint 2
pbdt1997 0:97879c8efe59 77 float readEnc2(){
pbdt1997 0:97879c8efe59 78 csEnc2 = 0;
pbdt1997 0:97879c8efe59 79 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 80 int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 81 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 82 csEnc2 = 1;
pbdt1997 0:97879c8efe59 83 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 84 }
pbdt1997 0:97879c8efe59 85
choyai 2:2ef793d0ac01 86 //ISR reads encoders every 20us
choyai 2:2ef793d0ac01 87 void readEncoders(){
choyai 2:2ef793d0ac01 88 q1 = readEnc1();
choyai 2:2ef793d0ac01 89 q2 = readEnc2();
choyai 2:2ef793d0ac01 90 }
choyai 2:2ef793d0ac01 91
pbdt1997 1:4eeae0f92c4c 92 //drive motor joint1
pbdt1997 1:4eeae0f92c4c 93 void drvMotor1(float duty){
pbdt1997 1:4eeae0f92c4c 94 if(duty > 0.9){
pbdt1997 1:4eeae0f92c4c 95 duty = 0.9;
pbdt1997 1:4eeae0f92c4c 96 }
pbdt1997 1:4eeae0f92c4c 97 else if(duty < -0.9){
pbdt1997 1:4eeae0f92c4c 98 duty = -0.9;
pbdt1997 1:4eeae0f92c4c 99 }
pbdt1997 1:4eeae0f92c4c 100 if(duty >= 0){
pbdt1997 1:4eeae0f92c4c 101 dir1 = 1; //CW
pbdt1997 1:4eeae0f92c4c 102
pbdt1997 1:4eeae0f92c4c 103 }
pbdt1997 1:4eeae0f92c4c 104 else if(duty < 0){
pbdt1997 1:4eeae0f92c4c 105 dir1 = 0; //CCW
pbdt1997 1:4eeae0f92c4c 106 duty = -duty;
pbdt1997 1:4eeae0f92c4c 107 }
pbdt1997 1:4eeae0f92c4c 108 pwm1.period_ms(1.0f);
pbdt1997 1:4eeae0f92c4c 109 pwm1.write(duty);
pbdt1997 1:4eeae0f92c4c 110 }
pbdt1997 1:4eeae0f92c4c 111
choyai 2:2ef793d0ac01 112 //drive motor joint2
pbdt1997 1:4eeae0f92c4c 113 void drvMotor2(float duty){
pbdt1997 1:4eeae0f92c4c 114 if(duty > 0.5){
pbdt1997 1:4eeae0f92c4c 115 duty = 0.5;
pbdt1997 1:4eeae0f92c4c 116 }
pbdt1997 1:4eeae0f92c4c 117 else if(duty < -0.5){
pbdt1997 1:4eeae0f92c4c 118 duty = -0.5;
pbdt1997 1:4eeae0f92c4c 119 }
pbdt1997 1:4eeae0f92c4c 120 if(duty >= 0){
choyai 2:2ef793d0ac01 121 dir2 = 0; //CCW
pbdt1997 1:4eeae0f92c4c 122 }
pbdt1997 1:4eeae0f92c4c 123 else if(duty < 0){
choyai 2:2ef793d0ac01 124 dir2 = 1; //CW
pbdt1997 1:4eeae0f92c4c 125 duty = -duty;
pbdt1997 1:4eeae0f92c4c 126 }
pbdt1997 1:4eeae0f92c4c 127 pwm2.period_ms(1.0f);
pbdt1997 1:4eeae0f92c4c 128 pwm2.write(duty);
pbdt1997 1:4eeae0f92c4c 129 }
pbdt1997 1:4eeae0f92c4c 130
pbdt1997 1:4eeae0f92c4c 131 //PID of joint 1
pbdt1997 1:4eeae0f92c4c 132 void PID_1(float pos){
pbdt1997 1:4eeae0f92c4c 133 pos = -pos;
pbdt1997 1:4eeae0f92c4c 134 const float Kp=0.5,Ki=0,Kd=0.1;
pbdt1997 1:4eeae0f92c4c 135 float u,e,p,s;
pbdt1997 1:4eeae0f92c4c 136 float offset = 110.66;
pbdt1997 1:4eeae0f92c4c 137 bool stop1 = false;
pbdt1997 1:4eeae0f92c4c 138
pbdt1997 1:4eeae0f92c4c 139 while(stop1==false){
choyai 2:2ef793d0ac01 140 printf("Encoder1: %.2f ", q1);
choyai 2:2ef793d0ac01 141 printf("Encoder2: %.2f\n", q2);
choyai 2:2ef793d0ac01 142 if(q1 < 110){
choyai 2:2ef793d0ac01 143 q1 = q1 + 360;
pbdt1997 1:4eeae0f92c4c 144 }
choyai 2:2ef793d0ac01 145 e = (pos - q1) + offset;
pbdt1997 1:4eeae0f92c4c 146 s = s + e;
pbdt1997 1:4eeae0f92c4c 147 if(abs(e)>0.5){
pbdt1997 1:4eeae0f92c4c 148 u = (Kp*e) + (Ki*s) + (Kd*(e-p));
pbdt1997 1:4eeae0f92c4c 149 }
pbdt1997 1:4eeae0f92c4c 150 else{
pbdt1997 1:4eeae0f92c4c 151 u = 0;
pbdt1997 1:4eeae0f92c4c 152 drvMotor1(0);
pbdt1997 1:4eeae0f92c4c 153 stop1 = true;
pbdt1997 1:4eeae0f92c4c 154 }
pbdt1997 1:4eeae0f92c4c 155 drvMotor1(u);
pbdt1997 1:4eeae0f92c4c 156 p = e;
pbdt1997 1:4eeae0f92c4c 157 }
pbdt1997 1:4eeae0f92c4c 158 }
pbdt1997 1:4eeae0f92c4c 159
pbdt1997 1:4eeae0f92c4c 160 //PID of joint 2
pbdt1997 1:4eeae0f92c4c 161 void PID_2(float pos){
pbdt1997 1:4eeae0f92c4c 162 const float Kp=1,Ki=0,Kd=0.1;
pbdt1997 1:4eeae0f92c4c 163 float u,e,p,s;
pbdt1997 1:4eeae0f92c4c 164 float offset = 21.52 - 135;
pbdt1997 1:4eeae0f92c4c 165 bool stop = false;
pbdt1997 1:4eeae0f92c4c 166 pos=-pos;
pbdt1997 1:4eeae0f92c4c 167 while(stop==false){
choyai 2:2ef793d0ac01 168 printf("Encoder1: %.2f ", q1);
choyai 2:2ef793d0ac01 169 printf("Encoder2: %.2f\n", q2);
choyai 2:2ef793d0ac01 170 e = (pos - q2) + offset;
pbdt1997 1:4eeae0f92c4c 171 s = s + e;
pbdt1997 1:4eeae0f92c4c 172 if(abs(e)>0.5){
pbdt1997 1:4eeae0f92c4c 173 u = (Kp*e) + (Ki*s) + (Kd*(e-p));
pbdt1997 1:4eeae0f92c4c 174 }
pbdt1997 1:4eeae0f92c4c 175 else{
pbdt1997 1:4eeae0f92c4c 176 u = 0;
pbdt1997 1:4eeae0f92c4c 177 drvMotor2(0);
pbdt1997 1:4eeae0f92c4c 178 stop = true;
pbdt1997 1:4eeae0f92c4c 179 }
pbdt1997 1:4eeae0f92c4c 180 drvMotor2(u);
pbdt1997 1:4eeae0f92c4c 181 p = e;
pbdt1997 1:4eeae0f92c4c 182 }
pbdt1997 1:4eeae0f92c4c 183 }
pbdt1997 1:4eeae0f92c4c 184 //Set Home
pbdt1997 1:4eeae0f92c4c 185 void setHome(){
pbdt1997 1:4eeae0f92c4c 186 PID_1(0);
pbdt1997 1:4eeae0f92c4c 187 PID_2(0);
pbdt1997 1:4eeae0f92c4c 188 }
pbdt1997 1:4eeae0f92c4c 189
pbdt1997 0:97879c8efe59 190 //Main
pbdt1997 0:97879c8efe59 191
pbdt1997 0:97879c8efe59 192 int main() {
choyai 2:2ef793d0ac01 193 encTicker.attach_us(&readEncoders, 2.0);
choyai 2:2ef793d0ac01 194
pbdt1997 1:4eeae0f92c4c 195 pc.baud(256000);
pbdt1997 0:97879c8efe59 196 RX_INT();
pbdt1997 0:97879c8efe59 197 pc.attach(&RX_INT, Serial::RxIrq);
pbdt1997 0:97879c8efe59 198
pbdt1997 0:97879c8efe59 199 csMstr = 1;
pbdt1997 0:97879c8efe59 200 csEnc1 = 1;
pbdt1997 0:97879c8efe59 201 csEnc2 = 1;
pbdt1997 0:97879c8efe59 202
pbdt1997 0:97879c8efe59 203 master.format(8,3);
pbdt1997 0:97879c8efe59 204 master.frequency(1000000);
pbdt1997 0:97879c8efe59 205 mstrEnc1.format(14, 3);
pbdt1997 0:97879c8efe59 206 mstrEnc1.frequency(500000);
pbdt1997 0:97879c8efe59 207 mstrEnc2.format(14, 3);
pbdt1997 0:97879c8efe59 208 mstrEnc2.frequency(500000);
pbdt1997 0:97879c8efe59 209
pbdt1997 0:97879c8efe59 210 //Program Loop
pbdt1997 0:97879c8efe59 211 while(true){
pbdt1997 0:97879c8efe59 212 if(getData == 1){
pbdt1997 0:97879c8efe59 213 for(char i = 0; i < arraySize; i++){
pbdt1997 0:97879c8efe59 214 pc.putc(array[i]);
pbdt1997 0:97879c8efe59 215 }
pbdt1997 0:97879c8efe59 216 if(array[2] == 122){ //receive 'z'
pbdt1997 0:97879c8efe59 217 start = true;
pbdt1997 0:97879c8efe59 218 csMstr = 0;
pbdt1997 0:97879c8efe59 219 master.write(array[3]);
pbdt1997 0:97879c8efe59 220 int masterData = master.write(0x00);
pbdt1997 0:97879c8efe59 221 csMstr = 1;
pbdt1997 0:97879c8efe59 222 pc.putc(masterData);
pbdt1997 0:97879c8efe59 223 pc.putc('A');
pbdt1997 1:4eeae0f92c4c 224 }
pbdt1997 1:4eeae0f92c4c 225 if(array[2] == 121){ //'y'
pbdt1997 1:4eeae0f92c4c 226 setHome();
pbdt1997 0:97879c8efe59 227 }
choyai 2:2ef793d0ac01 228 if(array[2] == 120){ //'x'
choyai 2:2ef793d0ac01 229 printf('q1 = %.2f, q2 = %.2)
pbdt1997 0:97879c8efe59 230
pbdt1997 0:97879c8efe59 231 getData = 0;
pbdt1997 0:97879c8efe59 232 }
pbdt1997 0:97879c8efe59 233
pbdt1997 0:97879c8efe59 234 }
pbdt1997 0:97879c8efe59 235
pbdt1997 0:97879c8efe59 236 }